/*
 Reprap firmware based on Sprinter
 Optimized for Sanguinololu 1.2 and above / RAMPS 
 
 This program is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.
 
 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.
 
 You should have received a copy of the GNU General Public License
 along with this program.  If not, see <http://www.gnu.org/licenses/>. */

/*
  This firmware is a mashup between Sprinter, grbl and parts from marlin.
  (https://github.com/kliment/Sprinter)
  
  Changes by Doppler Michael (midopple)
  
  Planner is from Simen Svale Skogsrud
  https://github.com/simen/grbl

  Parts of Marlin Firmware from ErikZalm
  https://github.com/ErikZalm/Marlin-non-gen6
  
  Sprinter Changelog
  -  Look forward function --> calculate 16 Steps forward, get from Firmaware Marlin and Grbl
  -  Stepper control with Timer 1 (Interrupt)
  -  Extruder heating with PID use a Softpwm (Timer 2) with 500 hz to free Timer1 for Steppercontrol
  -  command M220 Sxxx --> tune Printing speed online (+/- 50 %)
  -  G2 / G3 command --> circle function
  -  Baudrate set to 250 kbaud
  -  Testet on Sanguinololu Board
  -  M30 Command can delete files on SD Card
  -  move string to flash to free RAM vor forward planner
  -  M203 Temperature monitor for Repetier

 Version 1.3.04T
  - Implement Plannercode from Marlin V1 big thanks to Erik
  - Stepper interrupt with Step loops
  - Stepperfrequency 30 Khz
  - New Command
    * M202 - Set maximum feedrate that your machine can sustain (M203 X200 Y200 Z300 E10000) in mm/sec
    * M204 - Set default acceleration: S normal moves T filament only moves (M204 S3000 T7000) im mm/sec^2 
    * M205 - advanced settings:  minimum travel speed S=while printing T=travel only,  X= maximum xy jerk, Z=maximum Z jerk, E = max E jerk
  - Remove unused Variables
  - Check Uart Puffer while circle processing (CMD: G2 / G3)
  - Fast Xfer Function --> move Text to Flash
  - Option to deactivate ARC (G2/G3) function (save flash)
  - Removed modulo (%) operator, which uses an expensive divide

 Version 1.3.05T
  - changed homing function to not conflict with min_software_endstops/max_software_endstops (thanks rGlory)
  - Changed check in arc_func
  - Corrected distance calculation. (thanks jv4779)
  - MAX Feed Rate for Z-Axis reduced to 2 mm/s some Printers had problems with 4 mm/s
  
 Version 1.3.06T
 - the microcontroller can store settings in the EEPROM
 - M500 - stores paramters in EEPROM
 - M501 - reads parameters from EEPROM (if you need reset them after you changed them temporarily).
 - M502 - reverts to the default "factory settings". You still need to store them in EEPROM afterwards if you want to.
 - M503 - Print settings
 
 Version 1.3.07T
 - Optimize Variable Size (faster Code)
 - Remove unused Code from Interrupt --> faster ~ 22 us per step
 - Replace abs with fabs --> Faster and smaler
 - Add "store_eeprom.cpp" to makefile

 Version 1.3.08T
 - If a line starts with ';', it is ignored but comment_mode is reset.
   A ';' inside a line ignores just the portion following the ';' character.
   The beginning of the line is still interpreted.
   
 - Same fix for SD Card, tested and work

 Version 1.3.09T
 - Move SLOWDOWN Function up
 
 Version 1.3.10T
- Add info to GEN7 Pins
- Update pins.h for gen7, working setup for 20MHz
- calculate feedrate without extrude before planner block is set
- New Board --> GEN7 @ 20 Mhz …
- ENDSTOPS_ONLY_FOR_HOMING Option ignore Endstop always --> fault is cleared

 Version 1.3.11T
- fix for broken include in store_eeprom.cpp  --> Thanks to kmeehl (issue #145)
- Make fastio & Arduino pin numbering consistent for AT90USB128x. --> Thanks to lincomatic
- Select Speedtable with F_CPU
- Use same Values for Speedtables as Marlin 

 Version 1.3.12T
- Fixed arc offset.

 Version 1.3.13T
- Extrudemultiply with code M221 Sxxx (S100 original Extrude value)
- use Feedratefactor only when Extrude > 0
- M106 / M107 can drive the FAN with PWM + Port check for not using Timer 1
- Added M93 command. Sends current steps for all axis.
- New Option --> FAN_SOFT_PWM, with this option the FAN PWM can use every digital I/O

 Version 1.3.14T
- When endstop is hit count the virtual steps, so the print lose no position when endstop is hit

 Version 1.3.15T
- M206 - set additional homing offset 
- Option for minimum FAN start speed --> #define MINIMUM_FAN_START_SPEED  50  (set it to zero to deactivate)
  
 Version 1.3.16T
- Extra Max Feedrate for Retract (MAX_RETRACT_FEEDRATE)

 Version 1.3.17T
- M303 - PID relay autotune possible
- G4 Wait until last move is done

 Version 1.3.18T
- Problem with Thermistor 3 table when sensor is broken and temp is -20 °C

 Version 1.3.19T
- Set maximum acceleration. If "steps per unit" is Change the acc were not recalculated
- Extra Parameter for Max Extruder Jerk
- New Parameter (max_e_jerk) in EEPROM --> Default settings after update !

 Version 1.3.20T
- fix a few typos and correct english usage
- reimplement homing routine as an inline function
- refactor eeprom routines to make it possible to modify the value of a single parameter
- calculate eeprom parameter addresses based on previous param address plus sizeof(type)
- add 0 C point in Thermistortable 7

 Version 1.3.21T
- M301 set PID Parameter, and Store to EEPROM
- If no PID is used, deaktivate Variables for PID settings

 Version 1.3.22T
- Error in JERK calculation after G92 command is send, make problems 
  with Z-Lift function in Slic3r
- Add homing values can shown with M206 D

*/
#include "Configuration.h"

#if PRINTER_TYPE != PRINTER_MAXI_SLAVE

#include <avr/pgmspace.h>
#include <math.h>
#include <string.h>
#include <stdlib.h>
#include <assert.h>


#include "pins_config.h"
#include "digital_pins.h"
#include "io_config.h"
#include "Sprinter.h"
#include "speed_lookuptable.h"
#include "heater.h"
#include "HardwareSerial.h"
#include "heater.h"
#include "templates.h"
#include "timer0.h"
#include "PrinterSettings.h"
#include "asm_rutines.h"
#include "Serial.h"

#ifdef USE_ARC_FUNCTION
  #include "arc_func.h"
#endif

#ifdef SDSUPPORT
  #include "SdFat.h"
#endif






//=============================================================================================================

void st_init();
void st_synchronize();
void controllerFan();

//=============================================================================================================

void __cxa_pure_virtual(){};
	
	

// look here for descriptions of gcodes: http://linuxcnc.org/handbook/gcode/g-code.html
// http://objects.reprap.org/wiki/Mendel_User_Manual:_RepRapGCodes

//Implemented Codes
//-------------------
// G0  -> G1
// G1  - Coordinated Movement X Y Z E
// G2  - CW ARC
// G3  - CCW ARC
// G4  - Dwell S<seconds> or P<milliseconds>
// G10 - Set and send tool offsets [T<tool 0/1>] Xoffset Yoffset
// G28 - Home all Axis
// G90 - Use Absolute Coordinates
// G91 - Use Relative Coordinates
// G92 - Set current position to cordinates given

//RepRap M Codes
// M104 - Set extruder target temp [T<tool 0-1>] S<temp>
// M105 - Read current temp
// M106 - Fan on [T<tool 0-1>] [S<pwm 0-255>]
// M107 - Fan off [T<tool 0-1>]
// M109 - Wait for extruder current temp to reach target temp.
// M114 - Display current position

//Custom M Codes
// M20  - List SD card
// M21  - Init SD card
// M22  - Release SD card
// M23  - Select SD file (M23 filename.g)
// M24  - Start/resume SD print
// M25  - Pause SD print
// M26  - Set SD position in bytes (M26 S12345)
// M27  - Report SD print status
// M28  - Start SD write (M28 filename.g)
// M29  - Stop SD write
// M30  -  <filename> - Delete file on sd card
// NOT M31	- Send Fast transfer mode param
// NOT M32	- Fast transfer mode write to SD <filename>
// M42  - Set output on free pins, on a non pwm pin (over pin 13 on an arduino mega) use S255 to turn it on and S0 to turn it off. Use P to decide the pin (M42 P23 S255) would turn pin 23 on
// M80  - Turn on Power Supply
// M81  - Turn off Power Supply
// M82  - Set E codes absolute (default)
// M83  - Set E codes relative while in Absolute Coordinates (G90) mode
// M84  - Disable steppers until next move, 
//        or use S<seconds> to specify an inactivity timeout, after which the steppers will be disabled.  S0 to disable the timeout.
// M85  - Set inactivity shutdown timer with parameter S<seconds>. To disable set zero (default)
// M92  - Set axis_steps_per_unit - same syntax as G92
// M93  - Send axis_steps_per_unit
// M115	- Capabilities string
// M119 - Show Endstopper State 
// M140 - Set bed target temp
// M190 - Wait for bed current temp to reach target temp.
// M201 - Set maximum acceleration in units/s^2 for print moves (M201 X1000 Y1000)
// M202 - Set maximum feedrate that your machine can sustain (M203 X200 Y200 Z300 E10000) in mm/sec
// M203 - Set temperture monitor to Sx
// M204 - Set default acceleration: S normal moves T filament only moves (M204 S3000 T7000) in mm/sec^2
// M205 - advanced settings:  minimum travel speed S=while printing T=travel only,  X=maximum xy jerk, Z=maximum Z jerk
// M206 - set additional homing offset

// M220 - set speed factor override percentage S=factor in percent 
// M221 - set extruder multiply factor S100 --> original Extrude Speed 

// M301 - Set PID parameters P I and D
// M303 - PID relay autotune S<temperature> sets the target temperature. (default target temperature = 150C)

// M400 - Finish all moves

// M500 - stores paramters in EEPROM
// M501 - reads parameters from EEPROM (if you need to reset them after you changed them temporarily).
// M502 - reverts to the default "factory settings". You still need to store them in EEPROM afterwards if you want to.
// M503 - Print settings
// M504	- Restart to bootloader
// M505 - Change baud rate

// Debug feature / Testing the PID for Hotend
// M601 - Show Temp jitter from Extruder (min / max value from Hotend Temperature while printing)
// M602 - Reset Temp jitter from Extruder (min / max val) --> Don't use it while Printing
// M603 - Show Free Ram

// T0	- set tool 0 ,default
// T1	- set tool 1

#define _VERSION_TEXT "1.3.23T / 28.10.2013"

//Stepper Movement Variables
char axis_codes[NUM_AXIS] = {'X', 'Y', 'Z', 'E'};
bool axis_relative_modes[] = _AXIS_RELATIVE_MODES;
uint8_t active_extruder = 0;
unsigned long axis_steps_per_sqr_second[NUM_AXIS];
static float axis_jerk_steps[4];
unsigned long plateau_steps;  

//adjustable feed factor for online tuning printer speed
volatile int feedmultiply=100; //100->original / 200 -> Factor 2 / 50 -> Factor 0.5
int saved_feedmultiply;
volatile bool feedmultiplychanged=false;
volatile int extrudemultiply=100; //100->1 200->2

//boolean acceleration_enabled = false, accelerating = false;
//unsigned long interval;
float destination_pos_units[NUM_AXIS] = {0.0, 0.0, 0.0, 0.0};
float current_pos_units[NUM_AXIS] = {0.0, 0.0, 0.0, 0.0};
float add_homing[3]={0,0,0};
float inverse_axis_steps_per_unit[4];

static unsigned short virtual_steps_x = 0;
static unsigned short virtual_steps_y = 0;
static unsigned short virtual_steps_z = 0;

bool home_all_axis = true;
//unsigned ?? ToDo: Check
int16_t feedrate = 1500, saved_feedrate;

long gcode_N, gcode_LastN;
bool relative_mode = false;  //Determines Absolute or Relative Coordinates
bool is_homing = false;

#ifdef USE_ARC_FUNCTION
	//For arc center point coordinates, sent by commands G2/G3
	float offset[3] = {0.0, 0.0, 0.0};
#endif


///oscillation reduction
#ifdef RAPID_OSCILLATION_REDUCTION
  float cumm_wait_time_in_dir[NUM_AXIS]={0.0,0.0,0.0,0.0};
  bool prev_move_direction[NUM_AXIS]={1,1,1,1};
  float osc_wait_remainder = 0.0;
#endif

#if (MINIMUM_FAN_START_SPEED > 0)
  unsigned char fan_last_speed = 0;
  unsigned char fan_org_start_speed = 0;
  unsigned long previous_millis_fan_start = 0;
#endif

// comm variables and Commandbuffer
// BUFSIZE is reduced from 8 to 6 to free more RAM for the PLANNER
#define MAX_CMD_SIZE 96
#define BUFSIZE 6 //8
char cmdbuffer[BUFSIZE][MAX_CMD_SIZE];

bool fromsd[BUFSIZE];

//Need 1kb Ram --> only work with Atmega1284
#ifdef SD_FAST_XFER_AKTIV
  char fastxferbuffer[SD_FAST_XFER_CHUNK_SIZE + 1];
  int lastxferchar;
  long xferbytes;
#endif

unsigned char bufindr = 0;
unsigned char bufindw = 0;
unsigned char buflen = 0;
char serial_char;
int serial_count = 0;
uint8_t comment_mode = false;
char *strchr_pointer; // just a pointer to find chars in the cmd string like X, Y, Z, E, etc

//Send Temperature in °C to Host
int hotendtC = 0, bedtempC = 0;
       
//Inactivity shutdown variables
unsigned long previous_millis_cmd = 0;

//Temp Monitor for repetier
unsigned char manage_monitor = 255;

FORCE_INLINE void process_commands();
#ifdef USE_ARC_FUNCTION
	FORCE_INLINE void get_arc_coordinates();
#endif

uint8_t control_status;
uint32_t control_timer;

//------------------------------------------------
//Init the SD card 
//------------------------------------------------
#ifdef SDSUPPORT
  Sd2Card card;
  SdVolume volume;
  SdFile root;
  SdFile file;
  uint32_t filesize = 0;
  uint32_t sdpos = 0;
  bool sdmode = false;
  bool sdactive = false;
  bool savetosd = false;
  int16_t read_char_int;
  
OPTIMIZE_SIZE void initsd()
{
	sdactive = false;
	#ifdef SDSUPPORT
		SetPinMode(SD_EN_PIN, OUTPUT);
		WritePin(SD_EN_PIN, LOW);
	
		if(root.isOpen())
			root.close();
			
		//if (!card.init(SPI_HALF_SPEED,SDSS)){
		if (!card.init(SPI_FULL_SPEED))
		{
			//if (!card.init(SPI_HALF_SPEED,SDSS))
			Serial.print_P(PSTR("SD init fail\r\n"));
			USART1_flush_tx();
		}
		else if (!volume.init(&card))
          Serial.print_P(PSTR("volume.init failed\r\n"));
		else if (!root.openRoot(&volume)) 
          Serial.print_P(PSTR("Open root failed\r\n"));
		else
		{
			sdactive = true;
			print_disk_info();

			#ifdef SDINITFILE
			file.close();
			if(file.open(&root, "init.3df", O_READ))
			{
				sdpos = 0;
                filesize = file.fileSize();
                sdmode = true;
            }
			#endif
		}
    #endif
}
  
  #ifdef SD_FAST_XFER_AKTIV
  
  void fast_xfer()
  {
    char *pstr;
    boolean done = false;
    
    //force heater pins low
    if(HEATER_0_PIN > -1) WritePin(HEATER_0_PIN,LOW);
    if(HEATER_1_PIN > -1) WritePin(HEATER_1_PIN,LOW);
    
    lastxferchar = 1;
    xferbytes = 0;
    
    pstr = strstr(strchr_pointer+4, " ");
    
    if(pstr == NULL)
    {
      Serial.print_P(PSTR("invalid command\r\n"));
      return;
    }
    
    *pstr = '\0';
    
    //check mode (currently only RAW is supported
    if(strcmp(strchr_pointer+4, "RAW") != 0)
    {
      Serial.print_P(PSTR("Invalid transfer codec\r\n"));
      return;
    }else{
      Serial.print_P(PSTR("Selected codec: "));
      Serial.println(strchr_pointer+4);
    }
    
    if (!file.open(&root, pstr+1, O_CREAT | O_APPEND | O_WRITE | O_TRUNC))
    {
      Serial.print_P(PSTR("Open failed, File: "));
      Serial.print(pstr+1);
      Serial.print_P(PSTR("."));
    }else{
      Serial.print_P(PSTR("Writing to file: "));
      Serial.println(pstr+1);
    }
        
    Serial.print_P(PSTR("ok\r\n"));
    
    //RAW transfer codec
    //Host sends \0 then up to SD_FAST_XFER_CHUNK_SIZE then \0
    //when host is done, it sends \0\0.
    //if a non \0 character is recieved at the beginning, host has failed somehow, kill the transfer.
    
    //read SD_FAST_XFER_CHUNK_SIZE bytes (or until \0 is recieved)
    while(!done)
    {
      while(!Serial.available())
      {
      }
      if(Serial.read() != 0)
      {
        //host has failed, this isn't a RAW chunk, it's an actual command
        file.sync();
        file.close();
        return;
      }

      for(int i=0;i<SD_FAST_XFER_CHUNK_SIZE+1;i++)
      {
        while(!Serial.available())
        {
        }
        lastxferchar = Serial.read();
        //buffer the data...
        fastxferbuffer[i] = lastxferchar;
        
        xferbytes++;
        
        if(lastxferchar == 0)
          break;
      }
      
      if(fastxferbuffer[0] != 0)
      {
        fastxferbuffer[SD_FAST_XFER_CHUNK_SIZE] = 0;
        file.write(fastxferbuffer);
        Serial.print_P(PSTR("ok\r\n"));
      }else{
        Serial.print_P(PSTR("Wrote "));
        Serial.print(xferbytes);
        Serial.print_P(PSTR(" bytes.\r\n"));
        done = true;
      }
    }

    file.sync();
    file.close();
  }
  #endif
    

 OPTIMIZE_SIZE void print_disk_info(void)
 {

   // print the type of card
    Serial.print_P(PSTR("\nCard type: "));
    switch(card.type()) 
    {
      case SD_CARD_TYPE_SD1:
        Serial.print_P(PSTR("SD1\r\n"));
        break;
      case SD_CARD_TYPE_SD2:
        Serial.print_P(PSTR("SD2\r\n"));
        break;
      case SD_CARD_TYPE_SDHC:
        Serial.print_P(PSTR("SDHC\r\n"));
        break;
      default:
        Serial.print_P(PSTR("Unknown\r\n"));
    }
  
    //uint64_t freeSpace = volume.clusterCount()*volume.blocksPerCluster()*512;
    //uint64_t occupiedSpace = (card.cardSize()*512) - freeSpace;
    // print the type and size of the first FAT-type volume
    uint32_t volumesize;
    Serial.print_P(PSTR("\nVolume type is FAT"));
    Serial.println(volume.fatType(), DEC);
    
    volumesize = volume.blocksPerCluster(); // clusters are collections of blocks
    volumesize *= volume.clusterCount(); // we'll have a lot of clusters
    volumesize *= 512; // SD card blocks are always 512 bytes
    volumesize /= 1024; //kbytes
    volumesize /= 1024; //Mbytes
    Serial.print_P(PSTR("Volume size (Mbytes): "));
    Serial.println(volumesize);
   
    // list all files in the card with date and size
    //root.ls(LS_R | LS_DATE | LS_SIZE);
 }


  
  FORCE_INLINE void write_command(char *buf)
  {
      char* begin = buf;
      char* npos = 0;
      char* end = buf + strlen(buf) - 1;
      
      if((npos = strchr(buf, 'N')) != NULL)
      {
          begin = strchr(npos, ' ') + 1;
          end = strchr(npos, '*') - 1;
      }
      
      end[1] = '\r';
      end[2] = '\n';
      end[3] = '\0';
      
      
	  // Try write 5x before report error 
	  for(int i = 0; i < 5; i++)
	  {
	      file.writeError = false;
	      
          //Serial.println(begin);
          file.write(begin);
		  
		  if (!file.writeError)	break;
	  }
      
      if (file.writeError)
      {
          Serial.print_P(PSTR("error writing to file\r\n"));
      }
  }

#endif


OPTIMIZE_SIZE int FreeRam1(void)
{
  extern int  __bss_end;
  extern int* __brkval;
  int free_memory;

  if (reinterpret_cast<int>(__brkval) == 0)
  {
    // if no heap use from end of bss section
    free_memory = reinterpret_cast<int>(&free_memory) - reinterpret_cast<int>(&__bss_end);
  }
  else
  {
    // use from top of stack to heap
    free_memory = reinterpret_cast<int>(&free_memory) - reinterpret_cast<int>(__brkval);
  }
  
  return free_memory;
}

bool is_control_on();

FORCE_INLINE unsigned short calc_timer(unsigned short step_rate);

#include "gcode.h"
//------------------------------------------------
// Init 

#include <stdio.h>
//------------------------------------------------

OPTIMIZE_SIZE void SprinterSetup()
{ 
	while (!is_control_on());
	
	Serial.begin(16);		// spusteni seriove linky 1 pres USB
	
	//Serial.begin(BAUDRATE_NUM);
	Serial.println_P(PSTR("Sprinter"));
	Serial.println_P(PSTR(_VERSION_TEXT));
	Serial.println_P(PSTR("Start"));
		
	for(int i = 0; i < BUFSIZE; i++)
	{
		fromsd[i] = false;
	}
	
	#ifdef SDSUPPORT
		Serial.println_P(PSTR("SD Start"));
		initsd();
	#endif
	  
	Serial.println_P(PSTR("Planner init"));
	plan_init();  // Initialize planner;
	
	Serial.println_P(PSTR("Stepper init"));
	st_init();    // Initialize stepper
	
	
	
	//Free Ram
	Serial.print_P(PSTR("Free Ram: "));
	Serial.println(FreeRam1());
		
	//Planner Buffer Size
	Serial.print_P(PSTR("Plan Buffer Size:"));
	Serial.print((int)sizeof(block_t)*BLOCK_BUFFER_SIZE);
	Serial.print_P(PSTR(" / "));
	Serial.println(BLOCK_BUFFER_SIZE);
	  
	for(int8_t i=0; i < NUM_AXIS; i++)
	{
		axis_steps_per_sqr_second[i] = settings.max_acceleration_units_per_sq_second[i] * settings.axis_steps_per_unit[i];
		inverse_axis_steps_per_unit[i] = 1.0f / settings.axis_steps_per_unit[i];
	}
	
	// predpocitane hodnoty pro urychleni vypoctu
	axis_jerk_steps[0] = settings.max_xy_jerk * settings.axis_steps_per_unit[0];
	axis_jerk_steps[1] = settings.max_xy_jerk * settings.axis_steps_per_unit[1];
	axis_jerk_steps[2] = settings.max_z_jerk * settings.axis_steps_per_unit[2];
	axis_jerk_steps[3] = settings.max_e_jerk * settings.axis_steps_per_unit[3];
	
	settings.max_xy_jerk = fminf(JERK_DIV * inverse_axis_steps_per_unit[0], JERK_DIV * inverse_axis_steps_per_unit[1]);
	settings.max_z_jerk = JERK_DIV * inverse_axis_steps_per_unit[2];
	settings.max_e_jerk = JERK_DIV * inverse_axis_steps_per_unit[3];
	
//	all_drivers_enable_and_wakeup();

/*
	plan_buffer_line(-10.0, 0.0, 0.0, 0.0, 15000);
	plan_buffer_line(-20.0, 0.0, 0.0, 0.0, 15000);
	plan_buffer_line(-30.0, 0.0, 0.0, 0.0, 4000);
	plan_buffer_line(-40.0, 0.0, 0.0, 0.0, 4000);
	plan_buffer_line(-50.0, 0.0, 0.0, 0.0, 4000);
	plan_buffer_line(-60.0, 0.0, 0.0, 0.0, 4000);
	plan_buffer_line(-70.0, 0.0, 0.0, 0.0, 4000);
	plan_buffer_line(-80.0, 0.0, 0.0, 0.0, 4000);
	plan_buffer_line(-90.0, 0.0, 0.0, 0.0, 4000);
	plan_buffer_line(-100.0, 0.0, 0.0, 0.0, 4000);
	plan_buffer_line(0.0, 0.0, 0.0, 0.0, 4000);
*/
}


#ifdef CONTROL_BUTTON

extern  volatile uint8_t timer0_millis;

bool control;
bool control_aux;
uint8_t old_timer0;
uint8_t pb_filter;

OPTIMIZE_O3 bool is_control_on()
{
	uint8_t tmp = old_timer0 ^ timer0_millis;
	if ((tmp & 4) == 0)
		return control;
		
	old_timer0 = timer0_millis;
	
	pb_filter >>= 1;
	pb_filter |= (PINE & 0x80);
			
	if ( (pb_filter == 0x00) && !control_aux)
 		control_aux = true;
	
	if ( (pb_filter == 0xFF) & control_aux)
	{
		control = !control;
		
		if (control) 
		{
			turn_control_led_on();
			Serial.flush();
			Serial.println_P(PSTR("Printer on"));
			plan_init();
		} 
		else 
		{
			turn_control_led_off();
			kill();
			Serial.println_P(PSTR("Printer off"));
		}
			
		control_aux = false;
	}
	
	return control;
}
#else
	bool is_control_on() { return true; }
#endif

//------------------------------------------------
//MAIN LOOP
//------------------------------------------------
OPTIMIZE_O3 void SprinterLoop()
{
	if (is_control_on()) 
	{
		if(buflen < (BUFSIZE-1))
			get_command();
  
		if(buflen)
		{
			#ifdef SDSUPPORT
				if(savetosd)
				{
					if(strstr(cmdbuffer[bufindr],"M29") == NULL)
					{
						Serial.print_P(PSTR("ok\r\n"));
						write_command(cmdbuffer[bufindr]);
						//showString(PSTR("ok\r\n"));
					}
					else
					{
						file.sync();
						file.close();
						savetosd = false;
						Serial.println_P(PSTR("Done saving file."));
					}
				}
				else
				{
					process_commands();
				}
			#else
				process_commands();
			#endif

			buflen = (buflen-1);
			bufindr++;
			if(bufindr == BUFSIZE) bufindr = 0;
		}
  
		//check heater every n milliseconds
		manage_heaters();
		manage_inactivity();
	}
	
	controllerFan(); //Check if fan should be turned on to cool stepper drivers down
}

//------------------------------------------------
//Check Uart buffer while arc function ist calc a circle
//------------------------------------------------

void check_buffer_while_arc()
{
  if(buflen < (BUFSIZE-1))
  {
    get_command();
  }
}

//------------------------------------------------
//READ COMMAND FROM UART
//------------------------------------------------
OPTIMIZE_O3 void get_command() 
{ 
	while( Serial.available() > 0 && buflen < BUFSIZE)
	{
		serial_char = Serial.read();
		if(serial_char == '\n' || serial_char == '\r' || (serial_char == ':' && comment_mode == false) || serial_count >= (MAX_CMD_SIZE - 1) )
		{
			if(!serial_count) { //if empty line
			comment_mode = false; // for new command
			return;
		}
		cmdbuffer[bufindw][serial_count] = 0; //terminate string
		//Serial.println(cmdbuffer[bufindw]);

		fromsd[bufindw] = false;
		if(strstr(cmdbuffer[bufindw], "N") != NULL)
		{
			strchr_pointer = strchr(cmdbuffer[bufindw], 'N');
			gcode_N = (strtol(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL, 10));
			if(gcode_N != gcode_LastN+1 && (strstr(cmdbuffer[bufindw], "M110") == NULL) )
			{
				Serial.print_P(PSTR("Serial Error: Line Number is not Last Line Number+1, Last Line:"));
				Serial.println(gcode_LastN);
				//Serial.println(gcode_N);
				FlushSerialRequestResend();
				serial_count = 0;
				return;
			}
    
			if(strstr(cmdbuffer[bufindw], "*") != NULL)
			{
				uint8_t checksum = 0;
				uint8_t count = 0;
				while(cmdbuffer[bufindw][count] != '*') checksum = checksum^cmdbuffer[bufindw][count++];
				strchr_pointer = strchr(cmdbuffer[bufindw], '*');
  
				if( (int)(strtod(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL)) != checksum)
				{
					Serial.print_P(PSTR("Error: checksum mismatch, Last Line:"));
					Serial.println(gcode_LastN);
					FlushSerialRequestResend();
					serial_count = 0;
					return;
				}
			//if no errors, continue parsing
			} else  {
				Serial.print_P(PSTR("Error: No Checksum with line number, Last Line:"));
				Serial.println(gcode_LastN);
				FlushSerialRequestResend();
				serial_count = 0;
				return;
			}
    
			gcode_LastN = gcode_N;
			//if no errors, continue parsing
		}
		else  // if we don't receive 'N' but still see '*'
		{
			if((strstr(cmdbuffer[bufindw], "*") != NULL))
			{
				Serial.print_P(PSTR("Error: No Line Number with checksum, Last Line:"));
				Serial.println(gcode_LastN);
				serial_count = 0;
				return;
			}
		}
        
		if((strstr(cmdbuffer[bufindw], "G") != NULL))
		{
			strchr_pointer = strchr(cmdbuffer[bufindw], 'G');
			switch((int)((strtod(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL))))
			{
				case 0:
				case 1:
				#ifdef USE_ARC_FUNCTION
				case 2:  //G2
				case 3:  //G3 arc func
				#endif
					#ifdef SDSUPPORT
					if(savetosd)
					break;
					#endif
					Serial.println_P(PSTR("ok"));
					//Serial.println("ok"); 
				break;
            
				default:
					break;
			}
		}
		//Removed modulo (%) operator, which uses an expensive divide and multiplication
		//bufindw = (bufindw + 1)%BUFSIZE;
		bufindw++;
		if(bufindw == BUFSIZE) bufindw = 0;
		buflen += 1;

		comment_mode = false; //for new command
		serial_count = 0; //clear buffer
		} 
		// !!! skip zero chars => recieved after unpluged UART
		else if (serial_char == 0)
		{
			comment_mode = false; // for new command
			serial_count = 0; //clear buffer
		}
		else
		{
			if(serial_char == ';') comment_mode = true;
			if(!comment_mode) cmdbuffer[bufindw][serial_count++] = serial_char;
		}
	}// END while( Serial.available() > 0 && buflen < BUFSIZE)

	#ifdef SDSUPPORT
	if(!sdmode || serial_count!=0)
	{
		return;
	}
	while( filesize > sdpos && buflen < BUFSIZE)
	{
		serial_char = file.read();
		read_char_int = (int)serial_char;
    
		if(serial_char == '\n' || serial_char == '\r' || (serial_char == ':' && comment_mode == false) || serial_count >= (MAX_CMD_SIZE - 1) || read_char_int == -1) 
		{
			sdpos = file.curPosition();
			if(sdpos >= filesize)
			{
				sdmode = false;
				Serial.print_P(PSTR("Done printing file\r\n"));
			}
       
			if(!serial_count) { //if empty line
				comment_mode = false; // for new command
				return;
			}
        
			cmdbuffer[bufindw][serial_count] = 0; //terminate string

			fromsd[bufindw] = true;
			buflen += 1;
			//Removed modulo (%) operator, which uses an expensive divide and multiplication	
			//bufindw = (bufindw + 1)%BUFSIZE;
			bufindw++;
			if(bufindw == BUFSIZE) bufindw = 0;

			comment_mode = false; //for new command
			serial_count = 0; //clear buffer
		}
		else
		{
			if(serial_char == ';') comment_mode = true;
			if(!comment_mode) cmdbuffer[bufindw][serial_count++] = serial_char;
		}
	}
	#endif
}

//======================================================================================================================================

OPTIMIZE_O3 void get_command_new()
{
	while( Serial.available() > 0 && buflen < BUFSIZE)
	{
		serial_char = Serial.read();
		if(serial_char == '\n' || serial_char == '\r' || (serial_char == ':' && comment_mode == false) || serial_count >= (MAX_CMD_SIZE - 1) )
		{
			if(!serial_count) { //if empty line
				comment_mode = false; // for new command
				return;
			}
			cmdbuffer[bufindw][serial_count] = 0; //terminate string
			//Serial.println(cmdbuffer[bufindw]);

			fromsd[bufindw] = false;
			if(strstr(cmdbuffer[bufindw], "N") != NULL)
			{
				strchr_pointer = strchr(cmdbuffer[bufindw], 'N');
				gcode_N = (strtol(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL, 10));
				if(gcode_N != gcode_LastN+1 && (strstr(cmdbuffer[bufindw], "M110") == NULL) )
				{
					Serial.print_P(PSTR("Serial Error: Line Number is not Last Line Number+1, Last Line:"));
					Serial.println(gcode_LastN);
					//Serial.println(gcode_N);
					FlushSerialRequestResend();
					serial_count = 0;
					return;
				}
				
				if(strstr(cmdbuffer[bufindw], "*") != NULL)
				{
					uint8_t checksum = 0;
					uint8_t count = 0;
					while(cmdbuffer[bufindw][count] != '*') checksum = checksum^cmdbuffer[bufindw][count++];
					strchr_pointer = strchr(cmdbuffer[bufindw], '*');
					
					if( (int)(strtod(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL)) != checksum)
					{
						Serial.print_P(PSTR("Error: checksum mismatch, Last Line:"));
						Serial.println(gcode_LastN);
						FlushSerialRequestResend();
						serial_count = 0;
						return;
					}
					//if no errors, continue parsing
					} else  {
					Serial.print_P(PSTR("Error: No Checksum with line number, Last Line:"));
					Serial.println(gcode_LastN);
					FlushSerialRequestResend();
					serial_count = 0;
					return;
				}
				
				gcode_LastN = gcode_N;
				//if no errors, continue parsing
			}
			else  // if we don't receive 'N' but still see '*'
			{
				if((strstr(cmdbuffer[bufindw], "*") != NULL))
				{
					Serial.print_P(PSTR("Error: No Line Number with checksum, Last Line:"));
					Serial.println(gcode_LastN);
					serial_count = 0;
					return;
				}
			}
			
			if((strstr(cmdbuffer[bufindw], "G") != NULL))
			{
				strchr_pointer = strchr(cmdbuffer[bufindw], 'G');
				switch((int)((strtod(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL))))
				{
					case 0:
					case 1:
					#ifdef USE_ARC_FUNCTION
					case 2:  //G2
					case 3:  //G3 arc func
					#endif
					#ifdef SDSUPPORT
					if(savetosd)
					break;
					#endif
					Serial.println_P(PSTR("ok"));
					//Serial.println("ok");
					break;
					
					default:
					break;
				}
			}
			//Removed modulo (%) operator, which uses an expensive divide and multiplication
			//bufindw = (bufindw + 1)%BUFSIZE;
			bufindw++;
			if(bufindw == BUFSIZE) bufindw = 0;
			buflen += 1;

			comment_mode = false; //for new command
			serial_count = 0; //clear buffer
		}
		// !!! skip zero chars => recieved after unpluged UART
		else if (serial_char == 0)
		{
			comment_mode = false; // for new command
			serial_count = 0; //clear buffer
		}
		else
		{
			if(serial_char == ';') comment_mode = true;
			if(!comment_mode) cmdbuffer[bufindw][serial_count++] = serial_char;
		}
	}// END while( Serial.available() > 0 && buflen < BUFSIZE)

	#ifdef SDSUPPORT
	if(!sdmode || serial_count!=0)
	{
		return;
	}
	while( filesize > sdpos && buflen < BUFSIZE)
	{
		serial_char = file.read();
		read_char_int = (int)serial_char;
		
		if(serial_char == '\n' || serial_char == '\r' || (serial_char == ':' && comment_mode == false) || serial_count >= (MAX_CMD_SIZE - 1) || read_char_int == -1)
		{
			sdpos = file.curPosition();
			if(sdpos >= filesize)
			{
				sdmode = false;
				Serial.print_P(PSTR("Done printing file\r\n"));
			}
			
			if(!serial_count) { //if empty line
				comment_mode = false; // for new command
				return;
			}
			
			cmdbuffer[bufindw][serial_count] = 0; //terminate string

			fromsd[bufindw] = true;
			buflen += 1;
			//Removed modulo (%) operator, which uses an expensive divide and multiplication
			//bufindw = (bufindw + 1)%BUFSIZE;
			bufindw++;
			if(bufindw == BUFSIZE) bufindw = 0;

			comment_mode = false; //for new command
			serial_count = 0; //clear buffer
		}
		else
		{
			if(serial_char == ';') comment_mode = true;
			if(!comment_mode) cmdbuffer[bufindw][serial_count++] = serial_char;
		}
	}
	#endif
}

//============================================================================================================================================

static bool check_endstops = true;

void enable_endstops(bool check)
{
  check_endstops = check;
}

FORCE_INLINE float code_value() { return (strtod(&cmdbuffer[bufindr][strchr_pointer - cmdbuffer[bufindr] + 1], NULL)); }
FORCE_INLINE long code_value_long() { return (strtol(&cmdbuffer[bufindr][strchr_pointer - cmdbuffer[bufindr] + 1], NULL, 10)); }
FORCE_INLINE bool code_seen(char code_string[]) { return (strstr(cmdbuffer[bufindr], code_string) != NULL); }  //Return True if the string was found

FORCE_INLINE bool code_seen(char code)
{
  strchr_pointer = strchr(cmdbuffer[bufindr], code);
  return (strchr_pointer != NULL);  //Return True if a character was found
}

OPTIMIZE_SIZE void change_tool_routine(uint8_t tool, bool force)
{
	if (tool != active_extruder || force)
	{
		st_synchronize();
		int stored_feedrate = feedrate;
		feedrate = settings.change_tool_feedrate;
		active_extruder = tool;
		prepare_move();
		st_synchronize();
		feedrate = stored_feedrate;
	}	
}

OPTIMIZE_SIZE void homing_routine(uint8_t axis)
{
  int min_pin, max_pin, home_dir, max_length, home_bounce;

  switch(axis){
    case X_AXIS:
      min_pin = X_MIN_PIN;
      max_pin = X_MAX_PIN;
      home_dir = X_HOME_DIR;
      max_length = X_MAX_LENGTH;
      home_bounce = 10;
      break;
    case Y_AXIS:
      min_pin = Y_MIN_PIN;
      max_pin = Y_MAX_PIN;
      home_dir = Y_HOME_DIR;
      max_length = Y_MAX_LENGTH;
      home_bounce = 10;
      break;
    case Z_AXIS:
      min_pin = Z_MIN_PIN;
      max_pin = Z_MAX_PIN;
      home_dir = Z_HOME_DIR;
      max_length = Z_MAX_LENGTH;
      home_bounce = 4;
      break;
    default:
      //never reached
      break;
  }

  if ((min_pin > -1 && home_dir==-1) || (max_pin > -1 && home_dir==1))
  {
    current_pos_units[axis] = -1.5 * max_length * home_dir;
    plan_set_position(current_pos_units[X_AXIS], current_pos_units[Y_AXIS], current_pos_units[Z_AXIS], current_pos_units[E_AXIS]);
    destination_pos_units[axis] = 0;
    feedrate = settings.homing_feedrate[axis];
    prepare_move();
    st_synchronize();

    current_pos_units[axis] = home_bounce/2 * home_dir;
    plan_set_position(current_pos_units[X_AXIS], current_pos_units[Y_AXIS], current_pos_units[Z_AXIS], current_pos_units[E_AXIS]);
    destination_pos_units[axis] = 0;
    prepare_move();
    st_synchronize();

    current_pos_units[axis] = -home_bounce * home_dir;
    plan_set_position(current_pos_units[X_AXIS], current_pos_units[Y_AXIS], current_pos_units[Z_AXIS], current_pos_units[E_AXIS]);
    destination_pos_units[axis] = 0;
    feedrate = settings.homing_feedrate[axis]/2;
    prepare_move();
    st_synchronize();

    current_pos_units[axis] = (home_dir == -1) ? 0 : max_length;
    current_pos_units[axis] += add_homing[axis];
    plan_set_position(current_pos_units[X_AXIS], current_pos_units[Y_AXIS], current_pos_units[Z_AXIS], current_pos_units[E_AXIS]);
    destination_pos_units[axis] = current_pos_units[axis];
    feedrate = 0;
  }
}


//------------------------------------------------
// CHECK COMMAND AND CONVERT VALUES
//------------------------------------------------
OPTIMIZE_O3 void process_commands()
{
	unsigned long codenum; //throw away variable
	char *starpos = NULL;
	uint8_t extruder;

	if(code_seen('G'))
	{
		switch((int)code_value())
		{
			case 0: // G0 -> G1
			case 1: // G1
				#if (defined DISABLE_CHECK_DURING_ACC) || (defined DISABLE_CHECK_DURING_MOVE) || (defined DISABLE_CHECK_DURING_TRAVEL)
					manage_heaters();
				#endif
				get_coordinates(); // For X Y Z E F
				prepare_move();
				previous_millis_cmd = millis();
				//ClearToSend();
				return;
				//break;
				
			#ifdef USE_ARC_FUNCTION
			case 2: // G2  - CW ARC
				get_arc_coordinates();
				prepare_arc_move(true);
				previous_millis_cmd = millis();
				//break;
				return;
				
			case 3: // G3  - CCW ARC
				get_arc_coordinates();
				prepare_arc_move(false);
				previous_millis_cmd = millis();
				//break;
				return;  
			#endif
			  
			case 4: // G4 dwell
				codenum = 0;
				if(code_seen('P')) codenum = code_value(); // milliseconds to wait
				if(code_seen('S')) codenum = code_value() * 1000; // seconds to wait
				codenum += millis();  // keep track of when we started waiting
				st_synchronize();  // wait for all movements to finish
				while( millis()  < codenum )
				{
					manage_heaters();
				}
				break;
				

			// G10 - set tool offset
			case 10:
				st_synchronize();
				extruder = active_extruder;
				if (code_seen('T'))
					extruder = limit((uint8_t)code_value(), (uint8_t)0, (uint8_t)(EXTRUDERS_COUNT -1));

				if (code_seen('X'))
					settings.extruder_offsets[extruder][0] = code_value();

				if (code_seen('Y'))
					settings.extruder_offsets[extruder][1] = code_value();

				if (extruder == active_extruder)
					change_tool_routine(extruder, true);
				
				Serial.print_P(PSTR("Tool T"));
				Serial.print((long)extruder);
				Serial.print_P(PSTR(" ofssets X:"));
				Serial.print(settings.extruder_offsets[extruder][0]);
				Serial.print_P(PSTR(" Y:"));
				Serial.println(settings.extruder_offsets[extruder][1]);
				
				break;

			case 28: //G28 Home all Axis one at a time
				#ifdef FAN2_AS_CONTROLLERFAN
					set_fan2_pwm(255);
				#endif
				
				change_tool_routine(0, false);
			
				saved_feedrate = feedrate;
				saved_feedmultiply = feedmultiply;
				previous_millis_cmd = millis();
        
				feedmultiply = 100;    
      
				enable_endstops(true);
      
				for(int i=0; i < NUM_AXIS; i++) 
				{
					destination_pos_units[i] = current_pos_units[i];
				}
				feedrate = 0;
				is_homing = true;

				home_all_axis = !((code_seen(axis_codes[0])) || (code_seen(axis_codes[1])) || (code_seen(axis_codes[2])));

				if((home_all_axis) || (code_seen(axis_codes[Y_AXIS])))
				homing_routine(Y_AXIS);

				if((home_all_axis) || (code_seen(axis_codes[X_AXIS]))) 
					homing_routine(X_AXIS);

				if((home_all_axis) || (code_seen(axis_codes[Z_AXIS]))) 
					homing_routine(Z_AXIS);
        
				#ifdef ENDSTOPS_ONLY_FOR_HOMING
					enable_endstops(false);
      			#endif
      
				is_homing = false;
				feedrate = saved_feedrate;
				feedmultiply = saved_feedmultiply;
      
				previous_millis_cmd = millis();
				break;
				
			case 90: // G90
				relative_mode = false;
				break;
				
			case 91: // G91
				relative_mode = true;
				break;
				
			case 92: // G92
				//if(!code_seen(axis_codes[E_AXIS])) 
				st_synchronize();
          
				for(int i=0; i < NUM_AXIS; i++)
				{
					if(code_seen(axis_codes[i])) current_pos_units[i] = code_value();  
				}
				
				plan_set_position(current_pos_units[X_AXIS], current_pos_units[Y_AXIS], current_pos_units[Z_AXIS], current_pos_units[E_AXIS]);
				break;
		
			default:
				#ifdef SEND_WRONG_CMD_INFO
				Serial.print_P(PSTR("Unknown G-COM:"));
				 Serial.println(cmdbuffer[bufindr]);
				#endif
				break;
		} // END switch G code
	} // END if (code_seen('G'))

	else if(code_seen('M'))
	{
		switch( (int)code_value() ) 
		{
			#ifdef SDSUPPORT
			
			case 20: // M20 - list SD card
				Serial.println_P(PSTR("Begin file list"));
				root.ls();
				Serial.println_P(PSTR("End file list"));
				break;

			case 21: // M21 - init SD card
				sdmode = false;
				initsd();
				break;

			case 22: //M22 - release SD card
				sdmode = false;
				sdactive = false;
				break;

			case 23: //M23 - Select file
				if(sdactive)
				{
					sdmode = false;
					file.close();
					starpos = (strchr(strchr_pointer + 4,'*'));
            
					if(starpos!=NULL)
						*(starpos-1)='\0';
            
					if (file.open(&root, strchr_pointer + 4, O_READ)) 
					{
						Serial.print_P(PSTR("File opened:"));
						Serial.print(strchr_pointer + 4);
						Serial.print_P(PSTR(" Size:"));
						Serial.println(file.fileSize());
						sdpos = 0;
						filesize = file.fileSize();
						Serial.println_P(PSTR("File selected"));
					}
					else
					{
						Serial.print_P(PSTR("file.open failed"));
						Serial.println(card.errorCode());
						
					}
				} // end M23 if(sdactive)
				break;

			case 24: //M24 - Start SD print
				if(sdactive)
				{
					sdmode = true;
				}
				break;

			case 25: //M25 - Pause SD print
				if(sdmode)
				{
					sdmode = false;
				}
				break;

			case 26: //M26 - Set SD index
				if(sdactive && code_seen('S'))
				{
					sdpos = code_value_long();
					file.seekSet(sdpos);
				}
				break;
      
			case 27: //M27 - Get SD status
				if(sdactive)
				{
					Serial.print_P(PSTR("SD printing byte "));
					Serial.print(sdpos);
					Serial.print('/');
					Serial.println(filesize);
				}
				else
				{
					Serial.println_P(PSTR("Not SD printing"));
				}
				break;

			case 28: //M28 - Start SD write
				if(sdactive)
				{
					char* npos = 0;
					file.close();
					sdmode = false;
					starpos = (strchr(strchr_pointer + 4,'*'));
					if(starpos != NULL) {
						npos = strchr(cmdbuffer[bufindr], 'N');
						strchr_pointer = strchr(npos,' ') + 1;
						*(starpos-1) = '\0';
					}
            
					if (!file.open(root, strchr_pointer+4, O_CREAT | O_APPEND | O_WRITE | O_TRUNC)) {
						Serial.print_P(PSTR("open failed, File: "));
						Serial.print(strchr_pointer + 4);
						Serial.print_P(PSTR("."));
						Serial.println(card.errorCode());
					} else {
						savetosd = true;
						Serial.print_P(PSTR("Writing to file: "));
						Serial.println(strchr_pointer + 4);
					}
				}
				break; // END case M28

			case 29: //M29 - Stop SD write
				//processed in write to file routine above
				//savetosd = false;
				break;

			#ifndef SD_FAST_XFER_AKTIV
      
			case 30: // M30 filename - Delete file
				if(sdactive)
				{
					sdmode = false;
					file.close();
					starpos = (strchr(strchr_pointer + 4,'*'));
					
					if(starpos!=NULL)
					*(starpos-1)='\0';
					
					if(SdFile::remove(root, strchr_pointer + 4))
					{
						Serial.println_P(PSTR("File deleted"));
					}
					else
					{
						Serial.print_P(PSTR("Deletion failed:"));
						Serial.println(card.errorCode());
					}
				}
				break;
   
			#else     
      
			case 30: //M30 - fast SD transfer
				fast_xfer();
				break;

			case 31: //M31 - high speed xfer capabilities
				Serial.print_P(PSTR("RAW:"));
				Serial.println(SD_FAST_XFER_CHUNK_SIZE);
				break;
			
			#endif
        
		#endif
		      
			//M42 -Change pin status via gcode
			// P - pin number mode
			// S - value
			case 42:
				if (code_seen('P'))
				{
					uint8_t pin = code_value();

					for (uint8_t i = 0; i < sizeof(free_pins); ++i)
					{
						if (free_pins[i] == pin)
						{
							#ifdef CHAIN_OF_COMMAND
							st_synchronize(); // wait for all movements to finish
							#endif

							if (code_seen('S'))
							{
								uint8_t val = code_value();
								//SetPinModeAsm(pin, OUTPUT);
								//WritePinAsm(pin, val);
							}
						}
					}
				}
				break;

			// M104 - change target temp
			// T - extruder number[0,1], if no extruder selectd, active is used
			// S - target temp
			case 104:
				#ifdef CHAIN_OF_COMMAND
				st_synchronize(); // wait for all movements to finish
				#endif

				extruder = active_extruder;
				if (code_seen('T'))
					extruder = limit((uint8_t)code_value(), (uint8_t)0, (uint8_t)(EXTRUDERS_COUNT -1));

				if (code_seen('S')) {
					extruder_heaters[extruder].target_temp = code_value();
					extruder_heaters[extruder].target_raw = temp2analogh(extruder_heaters[extruder].target_temp);
				}
				break;

			// M140 set bed temp
			// S - new bed temp
			case 140:
				#ifdef CHAIN_OF_COMMAND
				st_synchronize(); // wait for all movements to finish
				#endif

				#ifdef BED_HEATER_ACTIVE
					if (code_seen('S')) {
						bed_heater.target_temp = code_value();
						bed_heater.target_raw = temp2analogBed(bed_heater.target_temp);
					}
				#endif
				break;

			// M105 - send temp extruder and bed
			// T - extruder[0,1]
			case 105: 
				extruder = active_extruder;
				if (code_seen('T'))
					extruder = limit((uint8_t)code_value(), (uint8_t)0, (uint8_t)(EXTRUDERS_COUNT -1));

				/*
				#ifndef BED_HEATER_ACTIVE
					printfn_P(USART1_write_byte, PSTR("ok T:%i @:%b\r\n"), extruder_heaters[extruder].current_temp, extruder_heaters[extruder].pwm); 
				#else
					printfn_P(USART1_write_byte, PSTR("ok T:%i @:%b B:%i\r\n"), extruder_heaters[extruder].current_temp, extruder_heaters[extruder].pwm, bed_heater.current_temp); 
				#endif
				*/
				Serial.print_P(PSTR("ok T:"));
				Serial.print(extruder_heaters[extruder].current_temp); 
				Serial.print_P(PSTR(" @:"));
				Serial.print((uint16_t)extruder_heaters[extruder].pwm);

				#ifdef BED_HEATER_ACTIVE
				Serial.print_P(PSTR(" B:"));
				Serial.println(bed_heater.current_temp); 
				#endif
			
				return;
 

			// M109 - Wait for extruder heater to reach target.
			// T extruder [0,1] if no specified, active is used
			// S new temp value
			case 109: 
			 	#ifdef CHAIN_OF_COMMAND
				st_synchronize(); // wait for all movements to finish
				#endif
				
				extruder = active_extruder;
				if (code_seen('T'))
					extruder = limit((uint8_t)code_value(), (uint8_t)0, (uint8_t)(EXTRUDERS_COUNT -1));

				if (code_seen('S')) 
				{
					extruder_heaters[extruder].target_temp = code_value();
					extruder_heaters[extruder].target_raw = temp2analogh(extruder_heaters[extruder].target_temp);
				}

				codenum = millis();
				while (!in_window(extruder_heaters[extruder].current_temp, extruder_heaters[extruder].target_temp, 2))
				{
					manage_heaters();
					if (millis() - codenum > 1000)
					{
						codenum = millis();
						Serial.print_P(PSTR("T:"));
						Serial.println(analog2temp(extruder_heaters[extruder].current_raw));
					}
				}
				break;
        
			// M190 - Wait for bed heater to reach target temperature.
			// S - new bed temperature
			case 190: 
				#ifdef CHAIN_OF_COMMAND
				st_synchronize(); // wait for all movements to finish
				#endif

				#ifdef BED_HEATER_ACTIVE
					if (code_seen('S')) {
						bed_heater.target_temp = code_value();
						bed_heater.target_raw = temp2analogBed(bed_heater.target_temp);
					}

					codenum = millis();
					while (bed_heater.current_temp < bed_heater.target_temp)
					{
						manage_heaters();

						if (millis() - codenum > 1000)
						{
							codenum = millis();
							
							Serial.print_P(PSTR("T:"));
							Serial.print(extruder_heaters[active_extruder].current_temp);
							Serial.print_P(PSTR(" B:"));
							Serial.println(bed_heater.current_temp);
						}
					}
				#endif
				break;

			// M106 Fan On
			// T - select extruder fan, or change active
			// S - select speed
			case 106: 
				#ifdef CHAIN_OF_COMMAND
				st_synchronize(); // wait for all movements to finish
				#endif

				extruder = active_extruder;
				if (code_seen('T'))
					extruder = limit((uint8_t)code_value(), (uint8_t)0, (uint8_t)(EXTRUDERS_COUNT -1));

				if (code_seen('S'))
					codenum = limit((uint8_t)code_value_long(), (uint8_t)0, uint8_t(255));
				else
					codenum = 255;

				if (extruder == 0)
					set_fan1_pwm(codenum);

				#ifndef FAN2_AS_CONTROLLERFAN
				else if (extruder == 1)
					set_fan2_pwm(codenum);
				#endif
				break;

			// M107 Fan Off
			case 107:
				extruder = active_extruder;
				if (code_seen('T'))
					extruder = limit((uint8_t)code_value(), (uint8_t)0, (uint8_t)(EXTRUDERS_COUNT -1));

				if (extruder == 0)
					set_fan1_pwm(0);

				#ifndef FAN2_AS_CONTROLLER_FAN
				else if (extruder == 1)
					set_fan2_pwm(0);
				#endif
				break; 

			// M82 - Set E axis absolute mode
			case 82:
				axis_relative_modes[3] = false;
				break;

			// M83 - Set E axis relative mode
			case 83:
				axis_relative_modes[3] = true;
				break;

			// M84 - Disable drives after last move
			// S - set new disable time
			// T - if T set enable drives, i T not set disable drives
			case 84:
				st_synchronize(); // wait for all movements to finish
				if(code_seen('S'))
				{
					settings.stepper_inactive_time = code_value() * 1000; 
				}
				else if(code_seen('T'))
				{
					enable_all_drivers();
				}
				else
				{ 
					disable_all_drivers();
				}
				break;

			// M85 - set max inactivity time im s
			case 85: 
				if (code_seen('S'))
					settings.max_inactive_time = code_value() * 1000; 
				break;

			// M92 - set axis step per units
			case 92: // M92
				for(int i=0; i < NUM_AXIS; i++) 
				{
					if(code_seen(axis_codes[i])) 
					{
						settings.axis_steps_per_unit[i] = code_value();
						inverse_axis_steps_per_unit[i] = 1.0f / settings.axis_steps_per_unit[i]; 
						axis_steps_per_sqr_second[i] = settings.max_acceleration_units_per_sq_second[i] * settings.axis_steps_per_unit[i];
					}
					
					settings.max_xy_jerk = fminf(JERK_DIV * inverse_axis_steps_per_unit[0], JERK_DIV * inverse_axis_steps_per_unit[1]);
					settings.max_z_jerk = JERK_DIV * inverse_axis_steps_per_unit[2];
					settings.max_e_jerk = JERK_DIV * inverse_axis_steps_per_unit[3];
					
					axis_jerk_steps[0] = settings.max_xy_jerk * settings.axis_steps_per_unit[0];
					axis_jerk_steps[1] = settings.max_xy_jerk * settings.axis_steps_per_unit[1];
					axis_jerk_steps[2] = settings.max_z_jerk * settings.axis_steps_per_unit[2];
					axis_jerk_steps[3] = settings.max_e_jerk * settings.axis_steps_per_unit[3];
				}
				break;
	      
			// M93 show current axis steps.
			// NO PARAMS
			case 93: 
				Serial.print_P(PSTR("ok "));
				Serial.print_P(PSTR("X:"));
				Serial.print(settings.axis_steps_per_unit[0]);
				Serial.print_P(PSTR(" Y:"));
				Serial.print(settings.axis_steps_per_unit[1]);
				Serial.print_P(PSTR(" Z:"));
				Serial.print(settings.axis_steps_per_unit[2]);
				Serial.print_P(PSTR(" E:"));
				Serial.println(settings.axis_steps_per_unit[3]);
				break;

			// M115 - print printer info
			case 115: 
				Serial.println_P(PSTR(_DEF_MACHINE_NAME));
                Serial.println_P(PSTR(_DEF_CHAR_UUID));
				break;

			// M114 - print current position
			case 114: // M114
				Serial.print_P(PSTR("X:"));
				Serial.print(current_pos_units[0]);
				Serial.print_P(PSTR(" Y:"));
				Serial.print(current_pos_units[1]);
				Serial.print_P(PSTR(" Z:"));
				Serial.print(current_pos_units[2]);
				Serial.print_P(PSTR(" E:"));
				Serial.println(current_pos_units[3]);
				break;
				
			// M119 - check endstops
			case 119: // M119
       			#if (X_MIN_PIN > -1)
					Serial.print_P(PSTR("x_min:"));
					Serial.print((ReadPin(X_MIN_PIN)^X_ENDSTOP_INVERT)?"H ":"L ");
      			#endif
      			#if (X_MAX_PIN > -1)
					Serial.print_P(PSTR("x_max:"));
					Serial.print((ReadPin(X_MAX_PIN)^X_ENDSTOP_INVERT)?"H ":"L ");
      			#endif
      			#if (Y_MIN_PIN > -1)
      				Serial.print_P(PSTR("y_min:"));
					Serial.print((ReadPin(Y_MIN_PIN)^Y_ENDSTOP_INVERT)?"H ":"L ");
      			#endif
      			#if (Y_MAX_PIN > -1)
      				Serial.print_P(PSTR("y_max:"));
					Serial.print((ReadPin(Y_MAX_PIN)^Y_ENDSTOP_INVERT)?"H ":"L ");
      			#endif
      			#if (Z_MIN_PIN > -1)
      				Serial.print_P(PSTR("z_min:"));
					Serial.print((ReadPin(Z_MIN_PIN)^Z_ENDSTOP_INVERT)?"H ":"L ");
      			#endif
      			#if (Z_MAX_PIN > -1)
      				Serial.print_P(PSTR("z_max:"));
					Serial.print((ReadPin(Z_MAX_PIN)^Z_ENDSTOP_INVERT)?"H ":"L ");
      			#endif
				Serial.println();
				break;

			// M201  Set maximum acceleration in units/s^2 for print moves (M201 X1000 Y1000)
			case 201: 
				for(int8_t i=0; i < NUM_AXIS; i++) 
				{
					if(code_seen(axis_codes[i]))
					{
						settings.max_acceleration_units_per_sq_second[i] = code_value();
						axis_steps_per_sqr_second[i] = code_value() * settings.axis_steps_per_unit[i];
					}
				}
				break;
      
			// M202 max feedrate mm/sec X,Y,Z,E
			case 202: 
				for(int8_t i=0; i < NUM_AXIS; i++) 
				{
					if(code_seen(axis_codes[i])) 
						settings.max_feedrate[i] = code_value();
				}
				break;
      
			// M203 Temperature monitor
			case 203: // M203 Temperature monitor
				if(code_seen('S')) 
					manage_monitor = code_value();
				
				if(manage_monitor==100) 
					manage_monitor=1; // Set 100 to heated bed
				break;

			// M204 set acceleration 
			// S - for normal moves 
			// T - for filmanent only moves
			case 204: 
				if(code_seen('S')) settings.move_acceleration = code_value() ;
				if(code_seen('T')) settings.retract_acceleration = code_value() ;
				break;

			//M205 advanced settings:  minimum travel speed S=while printing T=travel only,  B=minimum segment time X= maximum xy jerk, Z=maximum Z jerk, E= max E jerk
			case 205: 
				if(code_seen('S')) settings.minimumfeedrate = code_value();
				if(code_seen('T')) settings.mintravelfeedrate = code_value();
				//if(code_seen('B')) minsegmenttime = code_value() ;
				if(code_seen('X')) settings.max_xy_jerk = code_value() ;
				if(code_seen('Z')) settings.max_z_jerk = code_value() ;
				if(code_seen('E')) settings.max_e_jerk = code_value() ;
				break;

			// M206 - set additional homing offset
			// D - show XY homing offset
			case 206: 
				if(code_seen('D'))
				{
					Serial.print_P(PSTR("Addhome X:")); Serial.print(add_homing[0]);
					Serial.print_P(PSTR(" Y:")); Serial.print(add_homing[1]);
					Serial.print_P(PSTR(" Z:")); Serial.println(add_homing[2]);
				}

				for(int8_t cnt_i=0; cnt_i < 3; cnt_i++) 
				{
					if(code_seen(axis_codes[cnt_i])) 
						add_homing[cnt_i] = code_value();
				}
				break; 
				 
			// M220 S<factor in percent>- set speed factor override percentage
			case 220: 
				if(code_seen('S')) 
				{
					feedmultiply = code_value() ;
					feedmultiply = limit(feedmultiply, 20, 200);
					feedmultiplychanged=true;
				}
				break;
      
			// M221 S<factor in percent>- set extrude factor override percentage
			case 221: 
			{
				if(code_seen('S')) 
				{
					extrudemultiply = code_value() ;
					extrudemultiply = limit(extrudemultiply, 40, 200);
				}
			}
			break;

			// M301 - set pid regulator
			case 301: 
				if(code_seen('P')) settings.PID_Kp = code_value();
				if(code_seen('I')) settings.PID_Ki = code_value();
				if(code_seen('D')) settings.PID_Kd = code_value();
				updatePID();
				break;
			
			// M400 finish all moves
			case 400: 
				st_synchronize();	
				break;

			// M500 - Store settings in EEPROM
			case 500: 
				storeSettings();
				break;
        
			// M501 - Read stored settings from EEPROM
			case 501: 
				loadSettings(false);
				for(int8_t i=0; i < NUM_AXIS; i++)
				{
					axis_steps_per_sqr_second[i] = settings.max_acceleration_units_per_sq_second[i] * settings.axis_steps_per_unit[i];
				}
				break;
      
			// M502 - Revert to default settings
			case 502: 
				loadSettings(true);
				for(int8_t i=0; i < NUM_AXIS; i++)
				{
					axis_steps_per_sqr_second[i] = settings.max_acceleration_units_per_sq_second[i] * settings.axis_steps_per_unit[i];
				}
				break;
		
			// M503 - print current settings
			case 503: // print settings currently in memory
				printSettings();
				break;  
	  
			// M504 - restart to bootloader
			case 504:
				Serial.println_P(PSTR("Starting bootloader..."));
				USART1_flush_tx();
				EnableWDT(WDP_1S);
				while(1);
					manage_heaters();
				break;
				
			// M505 - change baud rate
			case 505:
				Serial.println_P(PSTR("Not implemented yet"));
				break;	
				
			case 603: // M603  Free RAM
				Serial.print_P(PSTR("Free Ram: "));
				Serial.println(FreeRam1()); 
				break;

			default:
				#ifdef SEND_WRONG_CMD_INFO
					Serial.print_P(PSTR("Unknown M-COM:"));
					Serial.println(cmdbuffer[bufindr]);
				#endif
				break;
		}
    
	}
	else if (code_seen('T'))
	{
		codenum = code_value();
		change_tool_routine(codenum, false);
	}
	else
	{
		Serial.print_P(PSTR("Unknown command:\r\n"));
		Serial.println(cmdbuffer[bufindr]);
	}
  
	ClearToSend();      
}

//============================================================================================

void FlushSerialRequestResend()
{
	Serial.flush();
	Serial.print_P(PSTR("Resend:"));
	Serial.println(gcode_LastN + 1);
	ClearToSend();
}

//========================================================================================

void ClearToSend()
{
	previous_millis_cmd = millis();
	#ifdef SDSUPPORT
		if(fromsd[bufindr])
			return;
	#endif
	Serial.println_P(PSTR("ok"));
}

//=======================================================================================

FORCE_INLINE void get_coordinates()
{
	for(int i=0; i < NUM_AXIS; i++)
	{
		if(code_seen(axis_codes[i])) 
			destination_pos_units[i] = (float)code_value() + (axis_relative_modes[i] || relative_mode)*current_pos_units[i];
		else 
			destination_pos_units[i] = current_pos_units[i];  //Are these else lines really needed?
	}
  
	if(code_seen('F'))
	{
		int32_t aux = code_value();
		
		// test overflow
		if (aux > 0x7fff)
			aux = 0x7fff;
		
		// test zero
		if(aux > 0) 
			feedrate = aux;
	}
}

#ifdef USE_ARC_FUNCTION
void get_arc_coordinates()
{
   get_coordinates();
   if(code_seen('I')) {
     offset[0] = code_value();
   } 
   else {
     offset[0] = 0.0;
   }
   if(code_seen('J')) {
     offset[1] = code_value();
   }
   else {
     offset[1] = 0.0;
   }
}
#endif



OPTIMIZE_O3 void prepare_move()
{
	long help_feedrate = 0;

	if(!is_homing)
	{
		if (min_software_endstops) 
		{
			if (destination_pos_units[X_AXIS] - settings.extruder_offsets[active_extruder][0] < 0) destination_pos_units[X_AXIS] = settings.extruder_offsets[active_extruder][0];
			if (destination_pos_units[Y_AXIS] - settings.extruder_offsets[active_extruder][1] < 0) destination_pos_units[Y_AXIS] = settings.extruder_offsets[active_extruder][1];
			if (destination_pos_units[Z_AXIS] < 0) destination_pos_units[Z_AXIS] = 0.0;
		}

		if (max_software_endstops) 
		{
			if (destination_pos_units[X_AXIS] > X_MAX_LENGTH) destination_pos_units[X_AXIS] = X_MAX_LENGTH;
			if (destination_pos_units[Y_AXIS] > Y_MAX_LENGTH) destination_pos_units[Y_AXIS] = Y_MAX_LENGTH;
			if (destination_pos_units[Z_AXIS] > Z_MAX_LENGTH) destination_pos_units[Z_AXIS] = Z_MAX_LENGTH;
		}
	}

	if(destination_pos_units[E_AXIS] > current_pos_units[E_AXIS])
	{
		help_feedrate = ((long)feedrate*(long)feedmultiply);
	}
	else
	{
		help_feedrate = ((long)feedrate*(long)100);
	}
  
	plan_buffer_line(destination_pos_units[X_AXIS] - settings.extruder_offsets[active_extruder][0], 
					 destination_pos_units[Y_AXIS] - settings.extruder_offsets[active_extruder][1], 
					 destination_pos_units[Z_AXIS], 
					 destination_pos_units[E_AXIS], 
					 help_feedrate/6000.0);
  
	for(int i=0; i < NUM_AXIS; i++)
	{
		current_pos_units[i] = destination_pos_units[i];
	} 
}


#ifdef USE_ARC_FUNCTION
void prepare_arc_move(char isclockwise) 
{

  float r = hypot(offset[X_AXIS], offset[Y_AXIS]); // Compute arc radius for mc_arc
  long help_feedrate = 0;

  if(destination_pos_units[E_AXIS] > current_pos_units[E_AXIS])
  {
    help_feedrate = ((long)feedrate*(long)feedmultiply);
  }
  else
  {
    help_feedrate = ((long)feedrate*(long)100);
  }

  // Trace the arc
  mc_arc(current_pos_units, destination_pos_units, offset, X_AXIS, Y_AXIS, Z_AXIS, help_feedrate/6000.0, r, isclockwise);
  
  // As far as the parser is concerned, the position is now == target. In reality the
  // motion control system might still be processing the action and the real tool position
  // in any intermediate location.
  for(int8_t i=0; i < NUM_AXIS; i++) 
  {
    current_pos_units[i] = destination_pos_units[i];
  }
}
#endif

OPTIMIZE_SIZE void kill()
{
	extruder_heaters[EXT1].target_temp = 0;
	set_heater1_pwm(0);
	
	// neovladam pwm heateru 2, pokud neni aktivni a pouziji ho pro kontrolku
	#ifdef EXT2_HEATER_ACTIVE
		extruder_heaters[EXT2].target_temp = 0;
		set_heater2_pwm(0);
	#endif	
		
	bed_heater.target_raw = 0;
	set_bed_heater_pwm(0);
		
	disable_all_drivers();
}

#ifdef FAN2_AS_CONTROLLERFAN
unsigned long lastMotor = 0; //Save the time for when a motor was turned on last
unsigned long lastMotorCheck = 0;

void controllerFan()
{
	if (millis() - lastMotorCheck >= 2500) //Not a time critical function, so we only check every 2500ms
	{
		lastMotorCheck = millis();
		
		uint8_t enable_mask = (PINC | 0xE0);

		if(enable_mask != 0xFF)
		{
			lastMotor = millis(); //... set time to NOW so the fan will turn on
		}
		
		if ((millis() - lastMotor) >= (CONTROLLERFAN_SEC*1000UL) || lastMotor == 0) //If the last time any driver was enabled, is longer since than CONTROLLERSEC...
		{
			set_fan2_pwm(0); //... turn the fan off
		}
		else
		{
			set_fan2_pwm(255); //... turn the fan on
		}
	}
}
#else

void controllerFan() {}

#endif

OPTIMIZE_O3 void manage_inactivity() 
{ 
	if( settings.max_inactive_time != 0)  
	{
		if( (millis() - previous_millis_cmd) >  settings.max_inactive_time ) 
			kill(); 
	}

	if( settings.stepper_inactive_time != 0)  
	{
		if( (millis() - previous_millis_cmd) >  settings.stepper_inactive_time )  {
			disable_all_drivers();
		}
	}
		
	#ifdef SLEEP_MODE_ON
		check_axes_activity();
	#endif
}

static block_t block_buffer[BLOCK_BUFFER_SIZE];			// A ring buffer for motion instructions buffer
static volatile block_t * block_buffer_head;			// Pointer to element for new movement
static volatile block_t * block_buffer_tail;			// Pointer to element actual movement
static volatile uint8_t block_buffer_size;

//===========================================================================
//=============================private variables ============================
//===========================================================================


// The current position of the tool in absolute steps
static long position_steps[4];   
static float previous_speed[4]; // Speed of previous path line segment
//static float prev_axis_ratio[4];

static float previous_nominal_speed; // Nominal speed of previous path line segment
static unsigned char G92_reset_previous_speed = 0;

// Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the 
// given acceleration:

FORCE_INLINE float estimate_acceleration_distance(float initial_rate, float target_rate, float acceleration)
{
	if (acceleration!=0) 
	{
		return((target_rate*target_rate - initial_rate*initial_rate) / (2.0*acceleration));
	}
	else 
	{
		return 0.0;  // acceleration was 0, set acceleration distance to 0
	}
}


// This function gives you the point at which you must start braking (at the rate of -acceleration) if 
// you started at speed initial_rate and accelerated until this point and want to end at the final_rate after
// a total travel of distance. This can be used to compute the intersection point between acceleration and
// deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed)


FORCE_INLINE float intersection_distance(float initial_rate, float final_rate, float acceleration, float _2ad)
{
	if (acceleration!=0)
	{
		return( (_2ad - initial_rate*initial_rate + final_rate*final_rate) / (4.0*acceleration) );
	}
	else
	{
		return 0.0;  // acceleration was 0, set intersection distance to 0
	}
}

// Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors.
// O1 - 4672 clocks
// O2 - 
// O3 - 4262 clocks
// O4 -
OPTIMIZE_O3 bool calculate_trapezoid_for_block(block_t *block, uint16_t initial_rate, uint16_t  final_rate)
{
	if (!block->recalculate_trapezoid_flag)
		return true;
	
	// Limit minimal step rate (Otherwise the timer will overflow.)
	if(initial_rate < 120)
		initial_rate=120;
		
	if(final_rate < 120) 
		final_rate=120;
	
	int32_t acceleration = block->acceleration_st;
	int32_t accelerate_steps = ceil(estimate_acceleration_distance(initial_rate, block->nominal_rate, acceleration));
	int32_t decelerate_steps = floor(estimate_acceleration_distance(final_rate, block->nominal_rate, acceleration));
	
	// Calculate the size of Plateau of Nominal Rate.
	int32_t plateau_steps = block->step_event_count - accelerate_steps - decelerate_steps;
	
	// Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
	// have to use intersection_distance() to calculate when to abort acceleration and start breaking
	// in order to reach the final_rate exactly at the end of this block.
	if (plateau_steps < 0)
	{
		accelerate_steps = ceil(intersection_distance(initial_rate, final_rate, acceleration, block->_2ad) );
		accelerate_steps = max(accelerate_steps, (int32_t)0); // Check limits due to numerical round-off
		accelerate_steps = min(accelerate_steps,(int32_t)block->step_event_count);
		plateau_steps = 0;
	}

	bool ret = false;
	asm volatile ("cli");  // Fill variables used by the stepper in a critical section
	if(block->busy == false) // Don't update variables if block is busy.
	{
		block->accelerate_until = accelerate_steps;
		block->decelerate_after = accelerate_steps+plateau_steps;
		block->initial_rate = initial_rate;
		block->final_rate = final_rate;
		ret = true;
	}
	asm volatile ("sei");
	
	block->recalculate_trapezoid_flag = false;
	
	return ret;
}

//================================================================================================
// optimalizovany vypocet maximalni dosazitelne rychlosti z pocatecni rychlosti
// param d2a je konstantni po celou dobu zivota bloku a tak je vypocten jednou a pote pouzit
FORCE_INLINE uint32_t max_allowable_rate(float initial_rate, float _2ad)
{
	return ceil(sqrt( (initial_rate * initial_rate) + _2ad ));
}

//================================================================================================

OPTIMIZE_O3 void planner_recalculate()
{
	if ( block_buffer_size < 3 )
		return;
	
	asm volatile ("cli");
	block_t * tail = (block_t*)block_buffer_tail;
	asm volatile ("sei");
	
	block_t *p = block_buffer_head->prev;		// ukazatel na posledni naplanovany pohyb, pred dalsi vkladany zaznam
	float allowable_initial = p->final_rate;	// povolena vstupni frekvence nasledujiciho bloku
		
	// reverse pass
	while (p != tail) 
	{
		// vypocet maximalni dosazitelne a povolene vstupni frekvence
		allowable_initial = fminf(p->max_initial_rate,	max_allowable_rate(allowable_initial, p->_2ad) );
		
		if ( allowable_initial == p->allowable_initial_rate )	// nova povolena vstupni frekvence je stejna jako minule
			break;												// takze pohyb jiz nelze dale efektivneji spojit a koncime
			
		p->allowable_initial_rate = allowable_initial;			// ulozeni nove povolene vstupni frekvence
		
		p = p->prev;	
	} 
	
	// forward pass
	float allowable_final;
	float prev_final = p->initial_rate;
	
	while( p->next != block_buffer_head )
	{
		// vypocet dosazitelne koncove frekvence
		allowable_final = fminf( p->nominal_rate, max_allowable_rate(prev_final, p->_2ad) );
			
		block_t *n = p->next; // ukazatel na nasledujici blok
			
		float junction_rate = fminf( allowable_final, n->allowable_initial_rate ); // nova frekvence prechodu
		
		// nova frekvence prechodu se zmenila
		if (junction_rate != p->final_rate)
		{
			p->recalculate_trapezoid_flag = true;
			if (calculate_trapezoid_for_block(p, prev_final, junction_rate) )	// prepocet aktualniho bloku
			{
				n->recalculate_trapezoid_flag = true;
				prev_final = junction_rate;
			}
			else
				prev_final = n->initial_rate;
		} 
		else
			prev_final = n->initial_rate;
		
		p = n;	
	};
	
	calculate_trapezoid_for_block(p, prev_final, p->final_rate);
}


OPTIMIZE_SIZE void plan_init() 
{
	for (uint8_t i = 0; i < BLOCK_BUFFER_MASK; ++i)
	{
		block_buffer[i].next = &block_buffer[i+1];
		block_buffer[i+1].prev = &block_buffer[i];
	}
	
	block_buffer[BLOCK_BUFFER_MASK].next = &block_buffer[0];
	block_buffer[0].prev = &block_buffer[BLOCK_BUFFER_MASK];
	
	block_buffer_head = &block_buffer[0];
	block_buffer_tail = block_buffer_head;
	block_buffer_size = 0;
	
	position_steps[0] = 0;
	position_steps[1] = 0;
	position_steps[2] = 0;
	position_steps[3] = 0;
	
	previous_speed[0] = 0.0;
	previous_speed[1] = 0.0;
	previous_speed[2] = 0.0;
	previous_speed[3] = 0.0;
	
	previous_nominal_speed = 0.0;
}

//=================================================================================================

//float junction_deviation = 0.1;
float max_E_feedrate_calc = MAX_RETRACT_FEEDRATE;
bool retract_feedrate_aktiv = false;

//============================================================================

FORCE_INLINE void check_axes_activity() 
{
	uint8_t out_bits = 0;
	
	cli();
	block_t * block = (block_t*)block_buffer_tail;
	sei();
	
	if(block != block_buffer_head) 
	{
		while(block != block_buffer_head) 
		{
			out_bits |= block->out_bits;
			block = block->next;
		}
	}

	if( (out_bits & 16) == 0) sleep_x();
	if( (out_bits & 32) == 0) sleep_y();
	if( (out_bits & 64) == 0) sleep_z();
	if( (out_bits & 128) == 0) sleep_e();
}


// Add a new linear movement to the buffer. steps_x, _y and _z is the absolute position in 
// mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration
// calculation the caller must also provide the physical length of the line in millimeters.
OPTIMIZE_O3 void plan_buffer_line(float x, float y, float z, float e, float feed_rate)
{
	// if buffer is full wait here
	while( block_buffer_size == BLOCK_BUFFER_MASK )
	{ 
		manage_heaters(); 
		manage_inactivity();  
	}
	
	enable_all_drivers();
	
	// The target position of the tool in absolute steps
	// Calculate target position in absolute steps
	//this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
	int32_t target_steps[4];
	target_steps[X_AXIS] = lroundf(x*settings.axis_steps_per_unit[X_AXIS]);
	target_steps[Y_AXIS] = lroundf(y*settings.axis_steps_per_unit[Y_AXIS]);
	target_steps[Z_AXIS] = lroundf(z*settings.axis_steps_per_unit[Z_AXIS]);     
	target_steps[E_AXIS] = lroundf(e*settings.axis_steps_per_unit[E_AXIS]);
  
	// Prepare to set up new block
	block_t *block = (block_t*)block_buffer_head;
  
	// Mark block as not busy (Not executed by the stepper interrupt)
	block->busy = false;

	block->out_bits = 0;

	int32_t delta_steps[4];
	// Number of steps for each axis and enable bits for each axis
	delta_steps[X_AXIS] = target_steps[X_AXIS] - position_steps[X_AXIS];
	block->steps_x = labs(delta_steps[X_AXIS]);
	if (block->steps_x != 0) {
		block->out_bits |= 16;	// x active
		wakeup_x();
	}

	delta_steps[Y_AXIS] = target_steps[Y_AXIS] - position_steps[Y_AXIS];
	block->steps_y = labs(delta_steps[Y_AXIS]);
	if (block->steps_y != 0) {
		block->out_bits |= 32;	// y active
		wakeup_y();
	}

	delta_steps[Z_AXIS] = target_steps[Z_AXIS] - position_steps[Z_AXIS];
	block->steps_z = labs(delta_steps[Z_AXIS]);
	if (block->steps_z != 0) {
		block->out_bits |= 64;	// z active
		wakeup_z();
	}
	
	delta_steps[E_AXIS] = target_steps[E_AXIS] - position_steps[E_AXIS];
	block->steps_e = labs(delta_steps[E_AXIS]);
	if (block->steps_e != 0) {
		block->out_bits |= 128;	// e active
		wakeup_e();
	}

	block->steps_e *= extrudemultiply;
	block->steps_e /= 100;
	block->step_event_count = max(max(block->steps_x, block->steps_y), max(block->steps_z, block->steps_e));

	// Bail if this is a zero-length block
	if (block->step_event_count <= dropsegments) { return; };
	
	// Compute direction bits for this block 
	if (delta_steps[X_AXIS] < 0) { block->out_bits |= 1; } 
	if (delta_steps[Y_AXIS] < 0) { block->out_bits |= 2; } 
	if (delta_steps[Z_AXIS] < 0) { block->out_bits |= 4; } 
	if (delta_steps[E_AXIS] < 0) 
	{ 
		block->out_bits |= 8; 
		//High Feedrate for retract
		max_E_feedrate_calc = MAX_RETRACT_FEEDRATE;
		retract_feedrate_aktiv = true;
	}
	else
	{
		if(retract_feedrate_aktiv)
		{
			if(block->steps_e > 0)
				retract_feedrate_aktiv = false;
		}
		else
		{
			max_E_feedrate_calc = settings.max_feedrate[E_AXIS]; 
		}
	}
  
	if (block->steps_e == 0) {
		if(feed_rate < settings.mintravelfeedrate) 
			feed_rate = settings.mintravelfeedrate;
	}
	else {
		if(feed_rate < settings.minimumfeedrate) 
			feed_rate = settings.minimumfeedrate;
	}

	float delta_mm[4];
	delta_mm[X_AXIS] = delta_steps[X_AXIS] * inverse_axis_steps_per_unit[X_AXIS];
	delta_mm[Y_AXIS] = delta_steps[Y_AXIS] * inverse_axis_steps_per_unit[Y_AXIS];
	delta_mm[Z_AXIS] = delta_steps[Z_AXIS] * inverse_axis_steps_per_unit[Z_AXIS];
	delta_mm[E_AXIS] = delta_steps[E_AXIS] * inverse_axis_steps_per_unit[E_AXIS];
  
	float millimeters;
	// compute travel distance in mm
	if ( block->steps_x <= dropsegments && block->steps_y <= dropsegments && block->steps_z <= dropsegments ) 
	{
		millimeters = fabsf(delta_mm[E_AXIS]);
	} 
	else 
	{
		millimeters = sqrtf(squaref(delta_mm[X_AXIS]) + squaref(delta_mm[Y_AXIS]) + squaref(delta_mm[Z_AXIS]));
	}
  
	float inverse_millimeters = 1.0/millimeters;  // Inverse millimeters to remove multiple divides 
	
	// Calculate speed in mm/second for each axis. No divide by zero due to previous checks.
	float inverse_second = feed_rate * inverse_millimeters;
  
	float nominal_speed = millimeters * inverse_second; // (mm/sec) Always > 0
	uint32_t nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0

	// limit factor derived from max frequency 30kHz 7.10.2013 Koron
	float speed_factor = fminf(1.0f, (float)MAX_STEP_FREQUENCY / nominal_rate);
		
	// Calculate and limit speed in mm/sec for each axis
	float current_speed[4];	
	for(int i=0; i < 3; i++) 
	{
		current_speed[i] = delta_mm[i] * inverse_second;
		if(fabsf(current_speed[i]) > settings.max_feedrate[i])
			speed_factor = fminf(speed_factor, settings.max_feedrate[i] / fabsf(current_speed[i]));
	}
  
	current_speed[E_AXIS] = delta_mm[E_AXIS] * inverse_second;
	if(fabsf(current_speed[E_AXIS]) > max_E_feedrate_calc)
		speed_factor = fminf(speed_factor, max_E_feedrate_calc / fabsf(current_speed[E_AXIS]));
		
	// slow down when the buffer starts to empty, rather than wait at the corner for a buffer refill
	uint8_t moves_queued = block_buffer_size;
	
	#ifdef SLOWDOWN
		if(moves_queued < (BLOCK_BUFFER_SIZE * 0.5) && moves_queued > 1) speed_factor = (speed_factor * moves_queued) / (BLOCK_BUFFER_SIZE * 0.5);
	#endif

	// Correct the speed  
	if( speed_factor < 1.0) 
	{
		current_speed[0] *= speed_factor;
		current_speed[1] *= speed_factor;
		current_speed[2] *= speed_factor;
		current_speed[3] *= speed_factor;
		nominal_speed *= speed_factor;
		nominal_rate *= speed_factor;
	}
	
	block->nominal_rate = nominal_rate;

	// pomery pro vypocet jerku
	float current_axis_ratios[4];
	float inverse_step_event_count = 1.0f / block->step_event_count;
	
	current_axis_ratios[X_AXIS] = (float)block->steps_x * inverse_step_event_count;
	current_axis_ratios[Y_AXIS] = (float)block->steps_y * inverse_step_event_count;
	current_axis_ratios[Z_AXIS] = (float)block->steps_z * inverse_step_event_count;
	current_axis_ratios[E_AXIS] = (float)block->steps_e * inverse_step_event_count;
	
	// Compute and limit the acceleration rate for the trapezoid generator.  
	float steps_per_mm = block->step_event_count * inverse_millimeters;
	if(block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0) 
	{
		block->acceleration_st = ceil(settings.retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
	}
	else 
	{
		block->acceleration_st = ceil(settings.move_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
		// Limit acceleration per axis
		if(((float)block->acceleration_st * current_axis_ratios[X_AXIS]) > axis_steps_per_sqr_second[X_AXIS])
			block->acceleration_st = axis_steps_per_sqr_second[X_AXIS];
			
		if(((float)block->acceleration_st * current_axis_ratios[Y_AXIS]) > axis_steps_per_sqr_second[Y_AXIS])
			block->acceleration_st = axis_steps_per_sqr_second[Y_AXIS];
			
		if(((float)block->acceleration_st * current_axis_ratios[E_AXIS]) > axis_steps_per_sqr_second[E_AXIS])
			block->acceleration_st = axis_steps_per_sqr_second[E_AXIS];
			
		if(((float)block->acceleration_st * current_axis_ratios[Z_AXIS]) > axis_steps_per_sqr_second[Z_AXIS])
			block->acceleration_st = axis_steps_per_sqr_second[Z_AXIS];
	}
	block->acceleration_rate = (long)(block->acceleration_st * 8.388608f);
  
		
	// Start with a safe speed
	float vmax_junction = settings.max_xy_jerk/2; 
	float vmax_junction_factor = 1.0; 

	if(fabs(current_speed[Z_AXIS]) > settings.max_z_jerk/2) 
		vmax_junction = fminf(vmax_junction, settings.max_z_jerk/2);

	if(fabs(current_speed[E_AXIS]) > settings.max_e_jerk/2) 
		vmax_junction = fminf(vmax_junction, settings.max_e_jerk/2);

	if(G92_reset_previous_speed == 1)
	{
		vmax_junction = 0.1;
		G92_reset_previous_speed = 0;  
	}

	vmax_junction = fminf(vmax_junction, nominal_speed);
	float safe_speed = vmax_junction;
	
	if ((moves_queued > 1) && (previous_nominal_speed > 0.0001)) 
	{
		float jerk = sqrt(square(current_speed[X_AXIS]-previous_speed[X_AXIS]) + square(current_speed[Y_AXIS]-previous_speed[Y_AXIS]));
		vmax_junction = nominal_speed;
		
		if (jerk > settings.max_xy_jerk) {
			vmax_junction_factor = (settings.max_xy_jerk/jerk);
		} 
		
		if(fabsf(current_speed[Z_AXIS] - previous_speed[Z_AXIS]) > settings.max_z_jerk) {
			vmax_junction_factor = fminf(vmax_junction_factor, (float)(settings.max_z_jerk/fabsf(current_speed[Z_AXIS] - previous_speed[Z_AXIS])));
		} 
		
		if(fabsf(current_speed[E_AXIS] - previous_speed[E_AXIS]) > settings.max_e_jerk) {
			vmax_junction_factor = fminf(vmax_junction_factor, (float)(settings.max_e_jerk/fabsf(current_speed[E_AXIS] - previous_speed[E_AXIS])));
		}
		 
		vmax_junction = fminf(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed
	}
	
	float fs_ratio = nominal_rate / nominal_speed;
	uint16_t if_rate = ceilf(safe_speed * fs_ratio);
	
	block->max_initial_rate = ceilf(fs_ratio * vmax_junction);
	block->_2ad = 2.0f * block->acceleration_st * block->step_event_count;
	
	// Update previous path unit_vector and nominal speed
	//memcpy(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[]
	previous_speed[0] = current_speed[0];
	previous_speed[1] = current_speed[1];
	previous_speed[2] = current_speed[2];
	previous_speed[3] = current_speed[3];
	previous_nominal_speed = nominal_speed;
	
	//prev_axis_ratio[0] = current_axis_ratios[0];
	//prev_axis_ratio[1] = current_axis_ratios[1];
	//prev_axis_ratio[2] = current_axis_ratios[2];
	//prev_axis_ratio[3] = current_axis_ratios[3];
  
	block->recalculate_trapezoid_flag = true;
	calculate_trapezoid_for_block(block, if_rate, if_rate);
	
	// Move buffer head
	block_buffer_head = block_buffer_head->next;
	asm volatile ("cli");
	++block_buffer_size;	// load, increment, store
	asm volatile ("sei");
  
	// Update position
	position_steps[0] = target_steps[0];
	position_steps[1] = target_steps[1];
	position_steps[2] = target_steps[2];
	position_steps[3] = target_steps[3];
	
	planner_recalculate();
}

#pragma GCC reset_options

//==================================================================================

OPTIMIZE_O3 void plan_set_position(float x, float y, float z, float e)
{
	position_steps[X_AXIS] = lround(x * settings.axis_steps_per_unit[X_AXIS]);
	position_steps[Y_AXIS] = lround(y * settings.axis_steps_per_unit[Y_AXIS]);
	position_steps[Z_AXIS] = lround(z * settings.axis_steps_per_unit[Z_AXIS]);     
	position_steps[E_AXIS] = lround(e * settings.axis_steps_per_unit[E_AXIS]);  

	virtual_steps_x = 0;
	virtual_steps_y = 0;
	virtual_steps_z = 0;

	previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
	previous_speed[0] = 0.0;
	previous_speed[1] = 0.0;
	previous_speed[2] = 0.0;
	previous_speed[3] = 0.0;
  
	G92_reset_previous_speed = 1;
}


//===========================================================================================
// Stepper
//===========================================================================================

// intRes = intIn1 * intIn2 >> 8
// charIn1 - 8bit
// intIn2 - 16bit
// result = unsigned 8 x unsigned 16 >> 8
// uses:
// r26 to store 0
#define MultiU16X8toH16(uint16_Res, uint8_In1, uint16_In2)	\
	asm volatile (											\
		"clr r26		\n\t"								\
		"mul %A1, %B2	\n\t"								\
		"movw %A0, r0	\n\t"								\
		"mul %A1, %A2	\n\t"								\
		"add %A0, r1	\n\t"								\
		"adc %B0, r26	\n\t"								\
		"lsl r0			\n\t"								\
		"adc %A0, r26	\n\t"								\
		"adc %B0, r26	\n\t"								\
		"clr r1			\n\t"								\
		: "=&r" (uint16_Res)								\
		: "d" (uint8_In1), "d" (uint16_In2)				\
		: "r26" )

FORCE_INLINE uint16_t mul_u16xu8touh16(uint8_t first, uint16_t second)
{
	uint16_t result;
	asm volatile ("clr	r26			\n\t"
				  "mul	%A1, %B2	\n\t"
				  "movw	%A0, r0		\n\t"
				  "mul	%A1, %A2	\n\t"
				  "add	%A0, r1		\n\t"
				  "adc	%B0, r26	\n\t"
				  "lsl	r0			\n\t"
				  "adc	%A0, r26	\n\t"
				  "adc	%B0, r26	\n\t"
				  "clr	r1			\n\t"
				  : "=&r" (result)
				  : "d" (first), "d" (second)
				  : "r26" );
			
	return result;	
}

//==========================================================================

// intRes = longIn1 * longIn2 >> 24
// LongIn1 - 24bit
// LongIn2 - 24bit
// Result = (uint16_t) ( LongIn1 * LongIn2 ) >> 24
// 16 = 24 * 24 >> 24
// If result overflow 16bit, then result is set to 0x7FFF = 32767
//
// uses:
// r25 to store 16bit result overflow
// r26 to store 0
// r27 to store the byte 1 of the 48bit result
#define MultiU24X24toH16limit(intRes, longIn1, longIn2) \
asm volatile ( \
"clr r26 \n\t" \
"clr r25		\n\t"	\
"mul %A1, %B2 \n\t" \
"mov r27, r1 \n\t" \
"mul %B1, %C2 \n\t" \
"movw %A0, r0 \n\t" \
"mul %C1, %C2 \n\t" \
"add %B0, r0 \n\t" \
"adc r25, r1	\n\t" \
"mul %C1, %B2 \n\t" \
"add %A0, r0 \n\t" \
"adc %B0, r1 \n\t" \
"adc r25, r26	\n\t" \
"mul %A1, %C2 \n\t" \
"add r27, r0 \n\t" \
"adc %A0, r1 \n\t" \
"adc %B0, r26 \n\t" \
"adc r25, r26	\n\t" \
"mul %B1, %B2 \n\t" \
"add r27, r0 \n\t" \
"adc %A0, r1 \n\t" \
"adc %B0, r26 \n\t" \
"adc r25, r26	\n\t" \
"mul %C1, %A2 \n\t" \
"add r27, r0 \n\t" \
"adc %A0, r1 \n\t" \
"adc %B0, r26 \n\t" \
"adc r25, r26	\n\t" \
"mul %B1, %A2 \n\t" \
"add r27, r1 \n\t" \
"adc %A0, r26 \n\t" \
"adc %B0, r26 \n\t" \
"adc r25, r26	\n\t"	\
"lsl r27 \n\t" \
"adc %A0, r26 \n\t" \
"adc %B0, r26 \n\t" \
"brmi	limit_%=	\n\t" \
"adc r25, r26	\n\t" \
"breq end_%=	\n\t"	\
"limit_%=:		\n\t"	\
"ldi %A0, 0xFF	\n\t"	\
"ldi %B0, 0x7F	\n\t"	\
"end_%=:		\n\t"	\
"clr r1 \n\t" \
: \
"=&d" (intRes) \
: \
"d" (longIn1), \
"d" (longIn2) \
: \
"r25", "r26" , "r27" \
)

// Some useful constants



#ifdef ENDSTOPS_ONLY_FOR_HOMING
  #define CHECK_ENDSTOPS  if(check_endstops)
#else
  #define CHECK_ENDSTOPS
#endif

static block_t *current_block;  // A pointer to the block currently being traced

// Variables used by The Stepper Driver Interrupt
static unsigned char out_bits;        // The next stepping-bits to be output
static long counter_x,       // Counter variables for the bresenham line tracer
            counter_y, 
            counter_z,       
            counter_e;
static int32_t step_events_completed; // The number of step events executed in the current block

static long acceleration_time, deceleration_time;
//static unsigned short acc_step_rate; // needed for deceleration start point
static uint16_t acc_step_rate;
static uint16_t step_rate;
static char step_loops;
static unsigned short OCR4A_nominal;

static volatile bool endstop_x_hit=false;
static volatile bool endstop_y_hit=false;
static volatile bool endstop_z_hit=false;

#if X_MIN_PIN > -1
	static bool old_x_min_endstop=false;
#endif

#if X_MAX_PIN > -1
	static bool old_x_max_endstop=false;
#endif

#if Y_MIN_PIN > -1
	static bool old_y_min_endstop=false;
#endif

#if Y_MAX_PIN > -1
	static bool old_y_max_endstop=false;
#endif

#if Z_MIN_PIN > -1
	static bool old_z_min_endstop=false;
#endif

#if Z_MAX_PIN > -1
	static bool old_z_max_endstop=false;
#endif



//         __________________________
//        /|                        |\     _________________         ^
//       / |                        | \   /|               |\        |
//      /  |                        |  \ / |               | \       s
//     /   |                        |   |  |               |  \      p
//    /    |                        |   |  |               |   \     e
//   +-----+------------------------+---+--+---------------+----+    e
//   |               BLOCK 1            |      BLOCK 2          |    d
//
//                           time ----->
// 
//  The trapezoid is the shape of the speed curve over time. It starts at block->initial_rate, accelerates 
//  first block->accelerate_until step_events_completed, then keeps going at constant speed until 
//  step_events_completed reaches block->decelerate_after after which it decelerates until the trapezoid generator is reset.
//  The slope of acceleration is calculated with the leib ramp alghorithm.


OPTIMIZE_O3 unsigned short calc_timer(unsigned short step_rate)
{
	unsigned short timer;
	if(step_rate > MAX_STEP_FREQUENCY) 
		step_rate = MAX_STEP_FREQUENCY;

	step_loops = 1;
	while (step_rate > 10000)
	{
		step_rate >>= 1;
		step_loops <<= 1;
	}  
	  
	if(step_rate < (F_CPU/500000)) 
		step_rate = 0;
	else
		step_rate -= (F_CPU/500000); // Correct for minimal speed
  
	if(step_rate >= (8*256)) // higher step rate 
	{ // higher step rate 
		unsigned short table_address = (unsigned short)&speed_lookuptable_fast[(unsigned char)(step_rate>>8)][0];
		unsigned char tmp_step_rate = (step_rate & 0x00ff);
		unsigned short gain = (unsigned short)pgm_read_word_near(table_address+2);
		MultiU16X8toH16(timer, tmp_step_rate, gain);
		//timer = mul_u16xu8touh16(tmp_step_rate, gain);
		timer = (unsigned short)pgm_read_word_near(table_address) - timer;
	}
	else 
	{ // lower step rates
		unsigned short table_address = (unsigned short)&speed_lookuptable_slow[0][0];
		table_address += ((step_rate)>>1) & 0xfffc;
		timer = (unsigned short)pgm_read_word_near(table_address);
		timer -= ( ((unsigned short)pgm_read_word_near(table_address+2) * (unsigned char)(step_rate & 0x0007)) >> 3);
	}
  
	if(timer < 100)
		timer = 100; //(20kHz this should never happen)
		
	return timer;
}


// Initializes the trapezoid generator from the current block. Called whenever a new 
// block begins.
FORCE_INLINE void trapezoid_generator_reset()
{
	deceleration_time = 0;
  
	// step_rate to timer interval
	OCR4A_nominal = calc_timer(current_block->nominal_rate); // 2.7.2013
	acc_step_rate = current_block->initial_rate;
	acceleration_time = calc_timer(acc_step_rate);
	OCR4A = acceleration_time;
}

// "The Stepper Driver Interrupt" - This timer interrupt is the workhorse.  
// It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately. 
ISR(TIMER4_COMPA_vect, OPTIMIZE_O1 )
{    
	// If there is no current block, attempt to pop one from the buffer
	if (current_block == NULL) 
	{
		// Anything in the buffer?
		//current_block = plan_get_current_block();
		if (block_buffer_size > 0)
			current_block = (block_t*)block_buffer_tail;
			
		if (current_block != NULL) 
		{
			trapezoid_generator_reset();
			counter_x = -(current_block->step_event_count >> 1);
			counter_y = counter_x;
			counter_z = counter_x;
			counter_e = counter_x;
			step_events_completed = 0;
			out_bits = current_block->out_bits;

			if ((out_bits & 1) != 0)
				set_x_dir(INVERT_X_DIR);
			else
				set_x_dir(!INVERT_X_DIR);

			if ((out_bits & 2) != 0)
				set_y_dir(INVERT_Y_DIR);
			else
				set_y_dir(!INVERT_Y_DIR);

			if ((out_bits & 4) != 0)
				set_z_dir(INVERT_Z_DIR);
			else
				set_z_dir(!INVERT_Z_DIR);

			if ((out_bits & 8) != 0) {
				set_e1_dir(INVERT_E_DIR);
				set_e2_dir(INVERT_E_DIR);
			} else {
				set_e1_dir(!INVERT_E_DIR);
				set_e2_dir(!INVERT_E_DIR);
			} 
		} 
		else 
		{
			OCR4A = 2000;
			
			// pouze pro hledani zamrznuti
			//OCR4A = 0xFFFF;
			//Serial.println(ad_h << 8 | ad_l);
		}    
	} // END if (current_block == NULL) 

	if (current_block != NULL) 
	{
		// Set direction and check limit switches
		if ((out_bits & 1) != 0) 
		{   // -direction
			CHECK_ENDSTOPS
			{
				#if X_MIN_PIN > -1
					bool x_min_endstop=(ReadPin(X_MIN_PIN) != X_ENDSTOP_INVERT);
					if(x_min_endstop && old_x_min_endstop && (current_block->steps_x > 0)) 
					{
						if(!is_homing)
							endstop_x_hit=true;
						else  
							step_events_completed = current_block->step_event_count;
					}
					else
					{
						endstop_x_hit=false;
					}
					old_x_min_endstop = x_min_endstop;
				#else
					endstop_x_hit=false;
				#endif
			}
		}
		else 
		{ // +direction 
			CHECK_ENDSTOPS 
			{
				#if X_MAX_PIN > -1
					bool x_max_endstop=(ReadPin(X_MAX_PIN) != X_ENDSTOP_INVERT);
					if(x_max_endstop && old_x_max_endstop && (current_block->steps_x > 0))
					{
						if(!is_homing)
							endstop_x_hit=true;
						else    
							step_events_completed = current_block->step_event_count;
					}
					else
					{
						endstop_x_hit=false;
					}
					old_x_max_endstop = x_max_endstop;
				#else
					endstop_x_hit=false;
				#endif
			}
		}

		if ((out_bits & 2) != 0) 
		{   // -direction
			CHECK_ENDSTOPS
			{
				#if Y_MIN_PIN > -1
					bool y_min_endstop=(ReadPin(Y_MIN_PIN) != Y_ENDSTOP_INVERT);
					if(y_min_endstop && old_y_min_endstop && (current_block->steps_y > 0)) 
					{
						if(!is_homing)
							endstop_y_hit=true;
						else
							step_events_completed = current_block->step_event_count;
					}
					else
					{
						endstop_y_hit=false;
					}
					old_y_min_endstop = y_min_endstop;
				#else
					endstop_y_hit=false;  
				#endif
			}
		}
		else 
		{ // +direction
			CHECK_ENDSTOPS
			{
				#if Y_MAX_PIN > -1
					bool y_max_endstop=( /*(bool)(PING & 0x02)*/ ReadPin(Y_MAX_PIN) != Y_ENDSTOP_INVERT );
					if(y_max_endstop && old_y_max_endstop && (current_block->steps_y > 0))
					{
						if(!is_homing)
							endstop_y_hit=true;
						else  
							step_events_completed = current_block->step_event_count;
					}
					else
					{
						endstop_y_hit=false;
					}
					old_y_max_endstop = y_max_endstop;
				#else
					endstop_y_hit=false;  
				#endif
				}
		}

		if ((out_bits & 4) != 0) 
		{   // -direction
			CHECK_ENDSTOPS
			{
				#if Z_MIN_PIN > -1
					bool z_min_endstop=(ReadPin(Z_MIN_PIN) != Z_ENDSTOP_INVERT);
					if(z_min_endstop && old_z_min_endstop && (current_block->steps_z > 0)) 
					{
						if(!is_homing)  
							endstop_z_hit=true;
						else  
							step_events_completed = current_block->step_event_count;
					}
					else
					{
						endstop_z_hit=false;
					}
					old_z_min_endstop = z_min_endstop;
				#else
					endstop_z_hit=false;  
				#endif
			}
		}
		else 
		{ // +direction
			CHECK_ENDSTOPS
			{
				#if Z_MAX_PIN > -1
					bool z_max_endstop=(ReadPin(Z_MAX_PIN) != Z_ENDSTOP_INVERT);
					if(z_max_endstop && old_z_max_endstop && (current_block->steps_z > 0)) 
					{
						if(!is_homing)
							endstop_z_hit=true;
						else  
							step_events_completed = current_block->step_event_count;
					}
					else
					{
						endstop_z_hit=false;
					}
					old_z_max_endstop = z_max_endstop;
				#else
					endstop_z_hit=false;  
				#endif
			}
		}

		for(int8_t i=0; i < step_loops; i++)  // Take multiple steps per interrupt (For high speed moves) 
		{
			if ((out_bits & 16) != 0)
			{
				counter_x += current_block->steps_x;
				if (counter_x > 0) 
				{
					if(!endstop_x_hit)
					{
						if(virtual_steps_x)
							virtual_steps_x--;
						else
							x_step_high();
					}
					else
						virtual_steps_x++;
          
					counter_x -= current_block->step_event_count;
					x_step_low();
				}
			}

			if ((out_bits & 32) != 0)
			{
				counter_y += current_block->steps_y;
				if (counter_y > 0) 
				{
					if(!endstop_y_hit)
					{
						if(virtual_steps_y)
							virtual_steps_y--;
						else
							y_step_high();
					}
					else
						virtual_steps_y++;
            
					counter_y -= current_block->step_event_count;
					y_step_low();
				}
			}

			if ((out_bits & 64) != 0)
			{
				counter_z += current_block->steps_z;
				if (counter_z > 0) 
				{
					if(!endstop_z_hit)
					{
						if(virtual_steps_z)
							virtual_steps_z--;
						else
							z_step_high();
					}
					else
						virtual_steps_z++;
          
					counter_z -= current_block->step_event_count;
					z_step_low();
				}
			}

			if ((out_bits & 128) != 0)
			{
				counter_e += current_block->steps_e;
				if (counter_e > 0) 
				{
					if (active_extruder == 0)
						e1_step_high();
					else if (active_extruder == 1)
						e2_step_high();

					counter_e -= current_block->step_event_count;
          
					e_step_low();
				}
			}
			
			step_events_completed += 1;  
			if(step_events_completed >= current_block->step_event_count) 
				break; 
		} // END for(int8_t i=0; i < step_loops; i++)

		// Calculate new timer value
		unsigned short timer;	
		if (step_events_completed <= current_block->accelerate_until) 
		{
			MultiU24X24toH16limit(acc_step_rate, acceleration_time, current_block->acceleration_rate);
			
			acc_step_rate += current_block->initial_rate;
			
			// upper limit
			if(acc_step_rate > current_block->nominal_rate)
				acc_step_rate = current_block->nominal_rate;

			// step_rate to timer interval
			timer = calc_timer(acc_step_rate);
			OCR4A = timer;
			acceleration_time += timer;
			
			//Serial.write('a');
			//Serial.println(acc_step_rate);
		} 
		else if (step_events_completed > current_block->decelerate_after) 
		{   
			MultiU24X24toH16limit(step_rate, deceleration_time, current_block->acceleration_rate);
				  
			if(step_rate > acc_step_rate) { // Check step_rate stays positive
				step_rate = current_block->final_rate;
			}
			else {
				step_rate = acc_step_rate - step_rate; // Decelerate from aceleration end point.
			}

			// lower limit
			if(step_rate < current_block->final_rate)
				step_rate = current_block->final_rate;

			// step_rate to timer interval
			timer = calc_timer(step_rate);
			OCR4A = timer;
			deceleration_time += timer;
			
			//Serial.write('d');
			//Serial.println(step_rate);
		}
		else 
		{
			OCR4A = OCR4A_nominal;
			//Serial.write('n');
			//Serial.println(current_block->nominal_rate);
		}

		// If current block is finished, reset pointer 
		if (step_events_completed >= current_block->step_event_count) 
		{
			current_block = NULL;
			block_buffer_tail = block_buffer_tail->next;
			--block_buffer_size;
			//plan_discard_current_block();
		}   
	} // END if (current_block != NULL) 
}

//=====================================================================

void st_init()
{
	#ifdef ENDSTOPS_ONLY_FOR_HOMING
		enable_endstops(false);
	#else
		enable_endstops(true);
	#endif
}

//=======================================================================

// Block until all buffered steps are executed
OPTIMIZE_SIZE void st_synchronize()
{
	while(block_buffer_size != 0) 
	{
		manage_heaters();
		manage_inactivity();
	}   
}

//=============================================================================
/*
#ifdef DEBUG
void log_message(char*   message) {
  Serial.print("DEBUG"); Serial.println(message);
}

void log_bool(char* message, bool value) {
  Serial.print("DEBUG"); Serial.print(message); Serial.print(": "); Serial.println(value);
}

void log_int(char* message, int value) {
  Serial.print("DEBUG"); Serial.print(message); Serial.print(": "); Serial.println(value);
}

void log_long(char* message, long value) {
  Serial.print("DEBUG"); Serial.print(message); Serial.print(": "); Serial.println(value);
}

void log_float(char* message, float value) {
  Serial.print("DEBUG"); Serial.print(message); Serial.print(": "); Serial.println(value);
}

void log_uint(char* message, unsigned int value) {
  Serial.print("DEBUG"); Serial.print(message); Serial.print(": "); Serial.println(value);
}

void log_ulong(char* message, unsigned long value) {
  Serial.print("DEBUG"); Serial.print(message); Serial.print(": "); Serial.println(value);
}

void log_int_array(char* message, int value[], int array_lenght) {
  Serial.print("DEBUG"); Serial.print(message); Serial.print(": {");
  for(int i=0; i < array_lenght; i++){
    Serial.print(value[i]);
    if(i != array_lenght-1) Serial.print(", ");
  }
  Serial.println("}");
}

void log_long_array(char* message, long value[], int array_lenght) {
  Serial.print("DEBUG"); Serial.print(message); Serial.print(": {");
  for(int i=0; i < array_lenght; i++){
    Serial.print(value[i]);
    if(i != array_lenght-1) Serial.print(", ");
  }
  Serial.println("}");
}

void log_float_array(char* message, float value[], int array_lenght) {
  Serial.print("DEBUG"); Serial.print(message); Serial.print(": {");
  for(int i=0; i < array_lenght; i++){
    Serial.print(value[i]);
    if(i != array_lenght-1) Serial.print(", ");
  }
  Serial.println("}");
}

void log_uint_array(char* message, unsigned int value[], int array_lenght) {
  Serial.print("DEBUG"); Serial.print(message); Serial.print(": {");
  for(int i=0; i < array_lenght; i++){
    Serial.print(value[i]);
    if(i != array_lenght-1) Serial.print(", ");
  }
  Serial.println("}");
}

void log_ulong_array(char* message, unsigned long value[], int array_lenght) {
  Serial.print("DEBUG"); Serial.print(message); Serial.print(": {");
  for(int i=0; i < array_lenght; i++){
    Serial.print(value[i]);
    if(i != array_lenght-1) Serial.print(", ");
  }
  Serial.println("}");
}
*/
#endif


